LidarMeasure/wj_lidar/measure_filter.h

/*
 * @Description: 测量结果滤波器
 * @Author: yct
 * @Date: 2021-03-15 14:41:46
 * @LastEditTime: 2021-03-16 16:30:08
 * @LastEditors: yct
 */

#ifndef MEASURE_FILTER_HH
#define MEASURE_FILTER_HH
#include "../tool/singleton.h"
#include "../tool/common_data.h"
#include "../error_code/error_code.h"
#include <map>
#include <deque>
#include <chrono>
#include <vector>
#include <algorithm>
#include "glog/logging.h"

#define FILTER_SIZE 10
#define MAX_QUEUE_SIZE 15
#define MAX_TIME_INTERVAL_MILLI 7000

class Measure_filter : public Singleton<Measure_filter>
{
    // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
    friend class Singleton<Measure_filter>;

public:
    // 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
    Measure_filter(const Measure_filter &) = delete;
    Measure_filter &operator=(const Measure_filter &) = delete;
    ~Measure_filter() = default;

    // 更新测量数据回调
    void update_data(int terminal_id, Common_data::Car_wheel_information_stamped data)
    {
        if(!data.wheel_data.correctness)
            return;
//        LOG(INFO) << data.wheel_data.to_string();
        // 未创建队列
        if (m_measure_results_map.find(terminal_id) == m_measure_results_map.end())
        {
            std::deque<Common_data::Car_wheel_information_stamped> t_deque;
            t_deque.push_back(data);
            m_measure_results_map.insert(std::pair<int, std::deque<Common_data::Car_wheel_information_stamped>>(terminal_id, t_deque));
//            LOG(INFO) <<"queue inserted";
        }
        else//已创建
        { 
            m_measure_results_map[terminal_id].push_back(data);
//            LOG(INFO) <<"data pushed "<<m_measure_results_map[terminal_id].size();
            // 剔除超时数据
            while(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now()-m_measure_results_map[terminal_id].front().measure_time).count() > MAX_TIME_INTERVAL_MILLI)
            {
                m_measure_results_map[terminal_id].pop_front();
            }
            // 维持队列长度
            while(m_measure_results_map[terminal_id].size() > MAX_QUEUE_SIZE)
            {
                m_measure_results_map[terminal_id].pop_front();
            }
        }
    }

    bool has_enough_data(int terminal_id)
    {
        if(m_measure_results_map.find(terminal_id) != m_measure_results_map.end() && m_measure_results_map[terminal_id].size() >= FILTER_SIZE)
            return true;
        else
            return false;
    }

    // 获取滤波后最新测量数据
    Error_manager get_filtered_wheel_information(int terminal_id, Common_data::Car_wheel_information& result)
    {
        // 检查数据量
        if(m_measure_results_map.find(terminal_id) == m_measure_results_map.end() || m_measure_results_map.find(terminal_id)->second.size() < FILTER_SIZE)
        {
            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR,
                    (std::string("缺少足够用于滤波的结果")+
                    std::to_string(terminal_id)+
                    std::string(m_measure_results_map.find(terminal_id) == m_measure_results_map.end()?
                        "end":
                        std::to_string(m_measure_results_map.find(terminal_id)->second.size()))).c_str());
        }
        // 填充待滤波结果到数组
        std::deque<Common_data::Car_wheel_information_stamped>* tp_deque = &m_measure_results_map.find(terminal_id)->second;
        std::vector<Common_data::Car_wheel_information_stamped> t_result_vec;
        Common_data::Car_wheel_information_stamped t_avg_result;
        for (std::deque<Common_data::Car_wheel_information_stamped>::reverse_iterator t_iter = tp_deque->rbegin(); t_iter != tp_deque->rend(); t_iter++)
        {
            t_result_vec.push_back(*t_iter);
            t_avg_result += *t_iter;
            if (t_result_vec.size() >= FILTER_SIZE)
                break;
        }
        if(t_result_vec.size()<FILTER_SIZE)
            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, "结果缺失");
        // 各结果计算平均,并打分
        t_avg_result /= t_result_vec.size();
        std::vector<float> t_score_vec, t_sorted_score_vec;
        for (size_t i = 0; i < t_result_vec.size(); i++)
        {
            t_score_vec.push_back((t_result_vec[i] - t_avg_result).calc_score());
            t_sorted_score_vec.push_back((t_result_vec[i] - t_avg_result).calc_score());
        }
        // 排序,丢掉最高两个(即与均值偏差最大两个,剩下平均获得最终值)
        std::sort(t_sorted_score_vec.begin(), t_sorted_score_vec.end());
        Common_data::Car_wheel_information_stamped t_final_result;
        int t_result_count = 0;
        for (size_t i = 0; i < t_score_vec.size(); i++)
        {
            if(t_score_vec[i] == t_sorted_score_vec[t_sorted_score_vec.size()-1] || t_score_vec[i] == t_sorted_score_vec[t_sorted_score_vec.size()-2])
            {
                continue;
            }else{
                t_final_result += t_result_vec[i];
                t_result_count++;
            }
        }
        if(t_result_count<=0)
            return Error_manager(WJ_FILTER_FLUCTUATING, MINOR_ERROR, "结果波动");
        t_final_result /= t_result_count;
        result = t_final_result.wheel_data;

//        LOG(INFO) << "\navg: \n\t"<<t_avg_result.wheel_data.to_string()<<"final: \t"<<t_final_result.wheel_data.to_string();
        return SUCCESS;
    }

private:
    // 父类的构造函数必须保护，子类的构造函数必须私有。
    Measure_filter() = default;

    // 各终端测量数据队列
    std::map<int, std::deque<Common_data::Car_wheel_information_stamped> > m_measure_results_map;
};

#endif // !MEASURE_FILTER_HH