DvsApsFusionProccessor.hpp

#ifndef DVS_APS_FUSION_PROCCESSOR_HPP
#define DVS_APS_FUSION_PROCCESSOR_HPP
#include "DvsenseBase/EventBase/EventTypes.hpp"
#include "DvsenseBase/ApsBase/ApsTypes.h"
#include "DvsenseBase/Utils/ObjectPool/DsObjectPoolFactory.h"
#include <queue>
#include <mutex>
 
#ifdef _WIN32
#define DVSENSE_API __declspec(dllexport)
#else
#define DVSENSE_API
#endif // _WIN32
namespace dvsense
{
    class DVSENSE_API DvsApsFusionProccessor
    {
    public:
        DvsApsFusionProccessor()
        {
            dvs_events_frame_pool_ =  DsObjectPoolFactory::createUniqueDsObjectPoolWithArgs<Event2DVector>(10, 3*1024*1024);
            dvs_events_buffer_ = dvs_events_frame_pool_->acquire();
            dvs_events_buffer_->clear();
            fusion_data_callback_ = nullptr;
        }
 
        void clear()
        {
            std::unique_lock<std::mutex> lock(fusion_queue_mutex_);
            while (!aps_frame_queue_.empty())
            {
                aps_frame_queue_.pop();
            }
            while (!dvs_events_frame_queue_.empty())
            {
                dvs_events_frame_queue_.pop();
            }
            dvs_events_buffer_->clear();
        }
 
        void addFusionDataCallback(DsFusionDataCallback cb)
        {
            fusion_data_callback_ = cb;
        }
        void removeFusionDataCallback()
        {
            fusion_data_callback_ = nullptr;
        }
 
        void addDvsData(const Event2D *begin, const Event2D *end)
        {
            dvs_events_buffer_->insert(dvs_events_buffer_->end(), begin, end);
        }
 
        void addApsData(const ApsFrame frame)
        {
            std::unique_lock<std::mutex> fusion_lock(fusion_queue_mutex_);
            aps_frame_queue_.push(frame.clone());
            fusionData();
        }
 
        void addSyncSignal(const EventTriggerIn &trigger_in)
        {
            if (trigger_in.polarity == 1)
            {
                std::unique_lock<std::mutex> fusion_lock(fusion_queue_mutex_);
                dvs_events_frame_queue_.push(dvs_events_buffer_);
                dvs_events_buffer_ = dvs_events_frame_pool_->acquire();
                dvs_events_buffer_->clear();
                fusionData();
            }
        }
 
        void fusionData()
        {
            while (!aps_frame_queue_.empty() && !dvs_events_frame_queue_.empty())
            {
                ApsFrame frame = aps_frame_queue_.front();
                aps_frame_queue_.pop();
                DsObjectPool<Event2DVector>::ptr_type dvs_events = dvs_events_frame_queue_.front();
                dvs_events_frame_queue_.pop();
                if (fusion_data_callback_)
                {
                    fusion_data_callback_(frame, dvs_events->data(), dvs_events->data() + dvs_events->size());
                }
            }
        }
 
    private:
        std::unique_ptr<DsObjectPool<Event2DVector>> dvs_events_frame_pool_;
        std::queue<DsObjectPool<Event2DVector>::ptr_type> dvs_events_frame_queue_;
        DsObjectPool<Event2DVector>::ptr_type dvs_events_buffer_;
 
        std::queue<ApsFrame> aps_frame_queue_;
        DsFusionDataCallback fusion_data_callback_;
        std::mutex fusion_queue_mutex_;
    };
} // namespace dvsense
 
#endif // DVS_APS_FUSION_PROCCESSOR_HPP