安全队列和曲线拟合

queue

template<typename DATATYPE, typename SEQUENCE = std::deque<DATATYPE>>
class ConcurrenceQueue {
public:
    ConcurrenceQueue() = default;
    ~ConcurrenceQueue() = default;
    ConcurrenceQueue & operator= (const ConcurrenceQueue &) = delete;
    /*
    ConcurrenceQueue(const ConcurrenceQueue & other) {
        std::lock_guard lg(other.m_mutex);
        m_data = other.m_data;
    }
    ConcurrenceQueue(ConcurrenceQueue &&) = delete;
    */
    bool empty() const {
        std::lock_guard<std::mutex> lg(m_mutex);
        return m_data.empty();
    }
    int size(){
        std::lock_guard<std::mutex>lg(m_mutex);
        return m_data.size();
    }
    int push(const DATATYPE & data) {
        std::lock_guard<std::mutex> lg(m_mutex);
        if(m_data.size()>maxSize)
            return 0;
        m_data.push(data);
        m_cond.notify_one();
        return m_data.size();
    }
    /*
    void push(DATATYPE && data) {
        std::lock_guard lg(m_mutex);
        m_data.push(std::move(data));
        m_cond.notify_one();
    }*/
    std::shared_ptr<DATATYPE> tryPop() {  // 非阻塞
        std::lock_guard<std::mutex> lg(m_mutex);
        if (m_data.empty()) return {};

        auto res = std::make_shared<DATATYPE>(m_data.front());
        m_data.pop();
        return res;
    }
    
    std::shared_ptr<DATATYPE> pop() {  // 阻塞
        std::unique_lock<std::mutex> lg(m_mutex);
        m_cond.wait(lg, [this] { return !m_data.empty(); });

        auto res = std::make_shared<DATATYPE>(std::move(m_data.front()));
        m_data.pop();
        return res;
    }
    std::shared_ptr<DATATYPE> pop(int sec) {  // 带超时的阻塞
        std::unique_lock<std::mutex> lg(m_mutex);

        bool notempty = m_cond.wait_for(lg, std::chrono::seconds(sec),[this] { return !m_data.empty(); });
        if(!notempty)//if(cs == std::cv_status::timeout || m_data.empty()) 
            return {};

        auto res = std::make_shared<DATATYPE>(std::move(m_data.front()));
        m_data.pop();
        return res;
    }
/*
    std::move唯一的功能是将一个左值强制转化为右值引用,继而可以通过右值引用使用该值,以用于移动语义。
    从实现上讲,std::move基本等同于一个类型转换:static_cast(lvalue);
    C++ 标准库使用比如vector::push_back 等这类函数时,会对参数的对象进行复制,连数据也会复制.
    这就会造成对象内存的额外创建, 本来原意是想把参数push_back进去就行了,通过std::move,可以避免不必要的拷贝操作。
*/
private:
    int maxSize=10;
    std::queue<DATATYPE, SEQUENCE> m_data;
    mutable std::mutex m_mutex;
    std::condition_variable m_cond;
};


fitline : obj mask拟合成一条三次曲线返回新的mask

cv::Mat getpoly(std::vector<cv::Point>& points, cv::Mat& binaryMask, std::string& linenode, bool fixpoints)
{
    Eigen::VectorXd x(points.size() );
    Eigen::VectorXd y(points.size() );
    for (size_t i = 0; i < points.size() ; ++i) {
        x(i) = points[i].x;
        y(i) = points[i].y;
    }

    // Perform polynomial curve fitting (3rd degree polynomial in this example)
    Eigen::VectorXd coefficients = Eigen::VectorXd::Zero(4);
    Eigen::MatrixXd A(points.size() , 4);
    for (size_t i = 0; i < points.size() ; ++i) {
        A(i, 0) = 1.0;
        A(i, 1) = x(i);
        A(i, 2) = x(i) * x(i);
        A(i, 3) = x(i) * x(i) * x(i);
    }

    coefficients = (A.transpose() * A).ldlt().solve(A.transpose() * y);

#ifdef MASK_DEBUGER
    // cv::Mat newMask = binaryMask;
    cv::Mat newMask = cv::Mat::zeros(binaryMask.size(), CV_8UC1);
    // linenode = std::to_string(coefficients(3)) + " " + std::to_string(coefficients(2)) + " " + std::to_string(coefficients(1));
    linenode = std::to_string(newMask.cols) + " " + std::to_string(newMask.rows) + " " + std::to_string(newMask.rows * newMask.cols);
#else
    cv::Mat newMask = cv::Mat::zeros(binaryMask.size(), CV_8UC1);
#endif
    if(std::abs(coefficients(3)) > 0.0001) return newMask;
    if(std::abs(coefficients(2)) > 0.0015 && std::abs(coefficients(1)) > 1.5) return newMask;
    for (int i = 0; i < newMask.cols; ++i) {
        int y = coefficients(0) + coefficients(1) * i +
                coefficients(2) * i * i +
                coefficients(3) * i * i * i;
        if (fixpoints && i > newMask.cols * 0.7) break;
        if (y >= 0 && y < newMask.rows) newMask.at<uchar>(y, i) = 255;
    }
#ifdef MASK_DEBUGER
    // cv::putText(newMask, linenode, linepoint, cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(255), 2);
    // cv::imwrite("/media/ubuntu/G/projects/net-2d/instance-seg/deploy/yolov8/ud_vision_detection_v8/test/" + linenode + ".jpg", newMask);
#endif
    return newMask;
}

你可能感兴趣的:(算法)