process线程作为estimator中的主要线程，对接收到的imu消息和image消息进行处理，进而通过这些信息估计出相机位姿。
1.process函数介绍
阅读IMU相关代码前，要能够明白IMU都能测量哪些数据。IMU就是惯性测量单元的缩写，主要用于测量加速度和旋转运动的传感器。基础的IMU包括加速度计和角速度计，一般由三轴的加速度传感器和三个轴的陀螺仪组成，可以测量IMU在三个轴方向上的线加速度和三个轴方向上的角速度信息。
process函数代码如下。
// 这个是处理measurements的线程
// thread: visual-inertial odometry
void process()
{
while (true)
{
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
std::unique_lock<std::mutex> lk(m_buf);
/**
* 这里执行条件变量con.wait函数并且measurements.size==0，就锁定了当前线程，当前线程会一直被阻塞
* 直到有另一个线程也使用con变量调用notification函数来唤醒当前线程
* 当其他线程调用了notification后，这里的wait函数中还需要判断(measurements = getMeasurements()).size() != 0是否为true,
* 只有当为true的时候才会解除当前线程的阻塞
* */
con.wait(lk, [&]
{
//1.获取在时间上“对齐”的IMU和图像数据的组合
return (measurements = getMeasurements()).size() != 0;
});
lk.unlock();
m_estimator.lock();
//2.遍历measurements，其实就是遍历获取每一个img_msg和其对应的imu_msg对数据进行处理
for (auto &measurement : measurements)
{
auto img_msg = measurement.second;
double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
//遍历和当前img_msg时间上“对齐”的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)//imu数据比图像数据早到
{
//第一次的时候current_time值为-1
if (current_time < 0)
current_time = t;
//计算时间间隔
double dt = t - current_time;
ROS_ASSERT(dt >= 0);
current_time = t;
//dx,dy,dz分别是IMU在三个轴方向上的加速度
dx = imu_msg->linear_acceleration.x;
dy = imu_msg->linear_acceleration.y;
dz = imu_msg->linear_acceleration.z;
//rx,ry,rz分别是IMU在三个轴方向上的角速度
rx = imu_msg->angular_velocity.x;
ry = imu_msg->angular_velocity.y;
rz = imu_msg->angular_velocity.z;
//对每一个IMU值进行预积分
estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
//printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);
}
else//图像数据比IMU数据早到
{
double dt_1 = img_t - current_time;
//t为imu的时间，dt_2表示imu消息的时间戳和图像数据时间戳之间的差值
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;
//对每一个IMU值进行预积分
estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
//printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz);
}
}
// set relocalization frame 设置重定位帧
sensor_msgs::PointCloudConstPtr relo_msg = NULL;
while (!relo_buf.empty())
{
relo_msg = relo_buf.front();
relo_buf.pop();
}
if (relo_msg != NULL)
{
vector<Vector3d> match_points;
double frame_stamp = relo_msg->header.stamp.toSec();
//遍历relo_msg中的points特征点
for (unsigned int i = 0; i < relo_msg->points.size(); i++)
{
Vector3d u_v_id;
u_v_id.x() = relo_msg->points[i].x;
u_v_id.y() = relo_msg->points[i].y;
u_v_id.z() = relo_msg->points[i].z;
match_points.push_back(u_v_id);
}
Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);
Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);
Matrix3d relo_r = relo_q.toRotationMatrix();
int frame_index;
frame_index = relo_msg->channels[0].values[7];
//设置重定位帧
estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
}
ROS_DEBUG("processing vision data with stamp %f \n", img_msg->header.stamp.toSec());
TicToc t_s;
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
//遍历img_msg中的特征点
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;
int camera_id = v % NUM_OF_CAM;
//获取img_msg中第i个点的x,y,z座标，这个是归一化后的座标值
double x = img_msg->points[i].x;
double y = img_msg->points[i].y;
double z = img_msg->points[i].z;
//获取像素的座标值
double p_u = img_msg->channels[1].values[i];
double p_v = img_msg->channels[2].values[i];
//获取像素点在x,y方向上的速度
double velocity_x = img_msg->channels[3].values[i];
double velocity_y = img_msg->channels[4].values[i];
ROS_ASSERT(z == 1);
Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
//建立每个特征点的image map,索引为feature_id
//image中每个特征点在帧中的位置信息和座标轴上的速度信息按照feature_id为索引存入image中
image[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}
//图像特征处理，包括初始化和非线性优化
estimator.processImage(image, img_msg->header);
double whole_t = t_s.toc();
printStatistics(estimator, whole_t);
std_msgs::Header header = img_msg->header;
header.frame_id = "world";
//给Rviz发送里程计信息：odometry、path、relo_path这几个topic
pubOdometry(estimator, header);
//给Rviz发送关键点位置
pubKeyPoses(estimator, header);
//给Rviz发送相机位置
pubCameraPose(estimator, header);
//给Rviz发送点云数据
pubPointCloud(estimator, header);
//发布tf转换topic
pubTF(estimator, header);
//发送关键帧信息
pubKeyframe(estimator);
if (relo_msg != NULL)
pubRelocalization(estimator);
//ROS_ERROR("conend: %f, at %f", img_msg->header.stamp.toSec(), ros::Time::now().toSec());
}
m_estimator.unlock();
m_buf.lock();
m_state.lock();
//3.NON_LINEAR情况下，调用update进行参数更新
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
update();
m_state.unlock();
m_buf.unlock();
}
}
总结来看，process中代码结构如下图所示：
其中，第二部分遍历measurements（其实就是遍历IMU数据和img_msg数据）进行处理的部分是process中最主要的部分，也是我要花时间和功夫重点研究的核心部分，这部分留到后边再慢慢说。这里先把第一部分和第三部分介绍一下。
2.getMeasurements函数解析
process函数中是一个大的while循环一直在运行，当线程创建后，进入线程入口函数process后在while(true)中首先就是将当前线程通过wait方法阻塞了，此时的(measurements = getMeasurements()).size() != 0取值是false，所以wait方法能够阻塞住当前线程。
其实这里说简单点就是生产者-消费者的问题，当生产者还没有生产出产品的时候消费者就在那里等着，等有产品的时候再工作。当前线程作为消费者，在当前线程运行后由于还没有IMU和图像数据，所以暂时将该线程阻塞住，等接收imu数据的回调函数和接收feature的回调函数中收到数据的时候再将该线程唤醒，这时候有数据了线程就可以执行了。
imu_callback和feature_callback中用于唤醒当前线程的语句如下，不清楚的可以返回 VINS-Mono代码阅读笔记（三）：vins_estimator中main函数分析 查看相关部分的代码。
con.notify_one();
getMeasurements函数主要工作就是将接收到的IMU数据和相关的图像数据进行“时间上的对齐”，并将对齐的IMU数据和图像数据进行组合供后边IMU和图像数据处理的时候使用。由于IMU的更新频率比相机要高出好多，所以和一帧图像时间上“对齐”的IMU数据是一个vector类型的容器中存放的IMU数据，在阅读代码的时候这个也需要知道。
getMeasurements函数代码如下：
//对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)
{
//imu信息接收到后会在imu_callback回调中存入imu_buf，feature消息收到后会在feature_callback中存入feature_buf中
if (imu_buf.empty() || feature_buf.empty())
return measurements;
//下面两个if判断都是对imu数据和image数据在时间戳上的判断（时间要对齐才能进行组合）
//imu_buf中最后一个imu消息的时间戳比feature_buf中第一个feature消息的时间戳还要小，说明imu数据发出来太早了
if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td))
{
//ROS_WARN("wait for imu, only should happen at the beginning");
//imu数据比图像数据要早，所以返回，等待时间上和图像对齐的imu数据到来
sum_of_wait++;
return measurements;
}
//imu_buf中第一个数据的时间戳比feature_buf的第一个数据的时间戳还要大
if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td))
{
//imu数据比图像数据滞后，所以图像数据要出队列
ROS_WARN("throw img, only should happen at the beginning");
feature_buf.pop();
continue;
}
//获取队列头部的图像数据，也就是最早发布的图像数据
sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();
feature_buf.pop();
std::vector<sensor_msgs::ImuConstPtr> IMUs;
//imu_buf的最前边的数据时间戳小于图像数据的时间戳的话，就将imu_buf中最前边的数据存入IMUs当中
while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)
{
//将imu_buf中最上边的元素插入到IMUs的尾部
IMUs.emplace_back(imu_buf.front());
imu_buf.pop();
}
//这行代码后难道不缺少一句imu_buf.pop();吗？？？
IMUs.emplace_back(imu_buf.front());
if (IMUs.empty())
ROS_WARN("no imu between two image");
//将和该img_msg时间上“对齐”的imu数据连同该img_msg放入measurements的尾部
//所以这里的measurements中存放的就是在时间上“对齐”的IMU数据和图像数据
measurements.emplace_back(IMUs, img_msg);
}
return measurements;
}
3.update函数
在process函数中遍历完measurements之后，说明当前接收到的IMU和图像数据已经处理完了，那么我们可以思考一下接下来做什么呢？因为后边还有可能继续接收IMU和图像数据，所以一些参数的初始值就变成了刚才处理完的数据的值（简单来说，因为IMU和图像数据是连续的，所以前一个状态的数值是后一个状态的初始值）。这个工作就是在update中来完成的。
update函数代码如下：
//得到窗口最后一个图像帧的imu项[P,Q,V,ba,bg,a,g]，对imu_buf中剩余imu_msg进行PVQ递推
void update()
{
TicToc t_predict;
latest_time = current_time;
tmp_P = estimator.Ps[WINDOW_SIZE];
tmp_Q = estimator.Rs[WINDOW_SIZE];
tmp_V = estimator.Vs[WINDOW_SIZE];
tmp_Ba = estimator.Bas[WINDOW_SIZE];
tmp_Bg = estimator.Bgs[WINDOW_SIZE];
acc_0 = estimator.acc_0;
gyr_0 = estimator.gyr_0;
queue<sensor_msgs::ImuConstPtr> tmp_imu_buf = imu_buf;
for (sensor_msgs::ImuConstPtr tmp_imu_msg; !tmp_imu_buf.empty(); tmp_imu_buf.pop())
predict(tmp_imu_buf.front());
}
其中的predict函数代码如下：
//从IMU测量值imu_msg和上一个PVQ递推得到当前PVQ
void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
double t = imu_msg->header.stamp.toSec();
//init_imu初始化的值为1
if (init_imu)
{
latest_time = t;
init_imu = 0;
return;
}
//计算当前imu_msg距离上一个imu_msg的时间间隔
double dt = t - latest_time;
latest_time = t;
//获取x,y,z三个方向上的线加速度
double dx = imu_msg->linear_acceleration.x;
double dy = imu_msg->linear_acceleration.y;
double dz = imu_msg->linear_acceleration.z;
Eigen::Vector3d linear_acceleration{dx, dy, dz};
//获取x,y,z三个方向上的角速度
double rx = imu_msg->angular_velocity.x;
double ry = imu_msg->angular_velocity.y;
double rz = imu_msg->angular_velocity.z;
Eigen::Vector3d angular_velocity{rx, ry, rz};
Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;
Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);
Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;
Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
//利用位移公式计算位移
tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;
//加速度情况下速度计算
tmp_V = tmp_V + dt * un_acc;
//线加速度
acc_0 = linear_acceleration;
//角速度
gyr_0 = angular_velocity;
}