Vins-Mono 论文 && Coding 一 3. visual-imu 数据同步

一、数据同步目的

对相邻相机时刻之间的 imu 数据预积分处理

二、同步流程

1. 缓存 buffer

(1). img features

void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg) {
    m_buf.lock();
    feature_buf.push(feature_msg);  // 压入 buffer
    m_buf.unlock();
    con.notify_one();   // 唤醒线程
}

(2). imu data

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg) {
    m_buf.lock();
    imu_buf.push(imu_msg);  // 压入 buffer
    m_buf.unlock();
    con.notify_one();  // 唤醒线程
}

2. 时间同步

// 返回: 多组 imu 数据对应一组 视觉特征
// 每一组: 多个 ts < img_ts 的 imu 以及第一个 >= img_ts 的 imu
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>>
getMeasurements()
{
    std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
    while (true)
    {
        // 某一类 buffer 为空, 同步失败
        if (imu_buf.empty() || feature_buf.empty())
            return measurements;
        // 最新的 imu 时间戳小于最旧的 img, 说明第一个 img 之前还有可能有 imu 数据, wiat
        if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td))  { 
            sum_of_wait++;
            return measurements;
        }
        // 保证提取的第一个视觉特征有对应的 imu 数据(imu 时间戳小于 img 时间戳)
        if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td))  { 
            feature_buf.pop();
            continue;
        }

        // 取 img features
        sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();
        feature_buf.pop();
        // 获取对应的 imu data, imu_ts < img_ts
        std::vector<sensor_msgs::ImuConstPtr> IMUs;
        while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)   { 
            IMUs.emplace_back(imu_buf.front());
            imu_buf.pop();
        }
        // 以及第一个 >= img_ts 的 imu 数据
        IMUs.emplace_back(imu_buf.front());
        if (IMUs.empty())
            ROS_WARN("no imu between two image");
        measurements.emplace_back(IMUs, img_msg);
    }
    return measurements;
}

3. 先后输入同步后的 imu, 视觉数据

(1). 输入 imu 数据

确保 estimator.processIMU() 处理的最后一个 imu 数据是同步/插值至图像时间戳的

  delta_t 是当前 imu 数据和上一帧 imu 数据的时间戳差值

for (auto &imu_msg : measurement.first) {
        double t = imu_msg->header.stamp.toSec();
        double img_t = img_msg->header.stamp.toSec() + estimator.td;
        if (t <= img_t) {  // 对于 ts <= img_ts 的 imu 数据, 计算 delta_t
          if (current_time < 0) current_time = t;
          double dt = t - current_time;
          current_time = t;
          dx = imu_msg->linear_acceleration.x;
          dy = imu_msg->linear_acceleration.y;
          dz = imu_msg->linear_acceleration.z;
          rx = imu_msg->angular_velocity.x;
          ry = imu_msg->angular_velocity.y;
          rz = imu_msg->angular_velocity.z;
          estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
        } else {  // 对于唯一一个大于 img_ts 的 imu 数据, 插值至 img_ts
          double dt_1 = img_t - current_time;
          double dt_2 = t - img_t;
          current_time = img_t;
          ROS_ASSERT(dt_1 >= 0);
          ROS_ASSERT(dt_2 >= 0);
          ROS_ASSERT(dt_1 + dt_2 > 0);
          double w1 = dt_2 / (dt_1 + dt_2);
          double w2 = dt_1 / (dt_1 + dt_2);
          dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
          dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
          dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
          rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
          ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
          rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
          estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
        }
      }

(2). 输入视觉数据

map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
// 封装视觉特征
for (unsigned int i = 0; i < img_msg->points.size(); i++) {
     int v = img_msg->channels[0].values[i] + 0.5;
     int feature_id = v / NUM_OF_CAM;  // track-id
     int camera_id = v % NUM_OF_CAM;
     double x = img_msg->points[i].x;  // 去畸变 u
     double y = img_msg->points[i].y;  // 去畸变 v
     double z = img_msg->points[i].z;  // 1.0
     double p_u = img_msg->channels[1].values[i]; // 去畸变前 u 分量
     double p_v = img_msg->channels[2].values[i]; // 去畸变前 v 分量
     double velocity_x = img_msg->channels[3].values[i];  // 光流速度 x 分量
     double velocity_y = img_msg->channels[4].values[i];  // 光流速度 y 分量
     Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
     xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
     image[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}
estimator.processImage(image, img_msg->header);