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;
}