一、数据同步目的
对相邻相机时刻之间的 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);