多传感器同步采样方法

多传感器的作用在自动驾驶，机器人领域应用广泛。 这里我根据工作讨论一种轻量级通用的多传感器同步采样的系统框架，可以用于算法研发，和轻量级产品的部署。下面以伪码的形式给出分析。

问题

我们假定三种传感器A, B, C; 采样频率 A>B>C。

不妨认为A -> imu (50-200hz), B ->camera (30-100hz), C -> lidar( 5-10hz),

需设计这样的一个框架， 数据连续输入时 C_k+1可以获取 B(C_k ~ C_k+1), 同时 任意一个B_t+1 可以获取  A(B_t ~B_t+1) , 处理好的数据便于后续算法使用和管理

假设

同时这又有三个强假设：

1 认为在5ms内的差异可以认为是对齐的。

2 认为A B C数据的时间戳为系统接收到的时间戳。其实传感器到系统还有一段延时，传感器接收到也自带时间戳。那样的时间戳配准涉及硬件的联动调试，会变得非常繁琐。

3 A B C数据的采集间隔稳定。

数据结构

// 输入

//用于存储接收的sensor数据

queue C_buf;

queue B_buf;

queue A_buf;

// 输出

queue measurements( pair( C, vector( pair(B , vector(A) )))

 

框架

首先，是数据接受处理，注意线程锁和信号同步

mutex m_buf;

condition_variable con;

//receive buffers

A_callback(&A_msg)
{
    m_buf.lock();
    A_buf.push(A_msg);
    m_buf.unlock();
    con.notify_one();//同时唤醒作用获取观测值数据的程序
}

B_callback(B_msg)
{ 
    m_buf.lock();
    B_buf.push(B_msg);
    m_buf.unlock();
    con.notify_one();
}

C_callback(C_msg)
{ 
    m_buf.lock();
    C_buf.push(C_msg);
    m_buf.unlock();
    con.notify_one();
}

prepareData()
{
    while (true)
    {
        if (A_buf.empty() || B_buf.empty() || C_buf.empty())
            return measurements;

        //considering the length of the A > B > C
        //先判断高频数据是否充足
        if(A_buf.back() ->stamp < B_buf.front() ->stamp)
            return measurements;

        if(B_buf.back() ->stamp < C_buf.front() ->stamp)
            return measurements;

        //
        if ((C_buf.front()->stamp > B_buf.front()->stamp))
        {
            C_buf.pop();
            continue;
        }

        if ((B_buf.front()->stamp > A_buf.front()->stamp))
        {
            B_buf.pop();
            continue;
        }
        
        //while 里层先从高频的累加， 逐步到外层低频的
        C_msg = C_buf.front();
        C_buf.pop();

        vector< pair(B_msg, vector(A_msg)> BA_vec;
        while (B_buf.front()->stamp <= C_msg->stamp)
        {

            B_msg = B_buf.front();
            B_buf.pop();
            vector<A_msg> A_vec;
            while (A_buf.front()->stamp <= B_msg->stamp)
            {
                A_vec.emplace_back(A_buf.front());
                A_buf.pop();
            }
            //对于独立stereo camera可以在此处添加
            //while...


            BA_vec.emplace_back(A_vec, B_buf.front());
            B_buf.pop();
        }

        measurements.emplace_back(BA_vec, C_msg);
    }
    return measurements; //
}

其次，在主函数中，定义好不同的线程

_receiveDataA()
{
    while(true)
    {
    receive( A_callback);
    }
}
//_receiveDataB, _receiveDataC ...同上可得

_processData()
{
    while (true)
    {
        unique_lock<mutex> lk(m_buf);
        //首先等待得到数据的代码在A_callback, B_callback, C_callback中被唤醒，当被唤醒后，得到measurements数据.得到数据这里调用了之前prepareData方法
        con.wait(lk, [&]
        {
            return (measurements = prepareData()).size() != 0;
        });
        lk.unlock();
     }
}

main()
{
    thread receiveDataA {  _receiveDataA};
    thread receiveDataB {  _receiveDataB};
    thread receiveDataC {  _receiveDataC};
    thread processData { _processData};
    thread algorithm0, algorithm1,  algorithm2 .....;//后面算法就可以加入自己的算法愉快的用measurements了
}