多传感器同步采样方法

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

问题

我们假定三种传感器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();//同时唤醒作用获取观测值数据的程序
}