目錄
- 代碼剖析
代碼剖析
首先一定要對Keyframe這個類有充分的認識，其中比較難了解就是一些pose的資料結構：
/** @brief vio pose in backend */
Eigen::Vector3d vio_T_w_i;
Eigen::Matrix3d vio_R_w_i;
/** @brief rectified pose in backend */
Eigen::Vector3d T_w_i;
Eigen::Matrix3d R_w_i;
/** @brief vio pose in frontend */
Eigen::Vector3d origin_vio_T;
Eigen::Matrix3d origin_vio_R;
- 第一個pose表示原始vio的位姿，也就是說直接保留前端發過來的pose，之後不做任何修改；
- 第二個pose表示經過後端優化之後的位于map坐标系的pose；
- 第三個pose表示被後端修改之後轉換到map坐标系的vio pose（沒有經過優化），這個要結合posegraph::addKeyFrame中的代碼了解，如下：
cur_kf->getVioPose(vio_P_cur, vio_R_cur);
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
...
if (cur_kf->findConnection(old_kf))
{
if (earliest_loop_index > loop_index || earliest_loop_index == -1)
earliest_loop_index = loop_index;
Vector3d w_P_old, w_P_cur, vio_P_cur;
Matrix3d w_R_old, w_R_cur, vio_R_cur;
old_kf->getVioPose(w_P_old, w_R_old);
cur_kf->getVioPose(vio_P_cur, vio_R_cur);
Vector3d relative_t;
Quaterniond relative_q;
relative_t = cur_kf->getLoopRelativeT();
relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();
w_P_cur = w_R_old * relative_t + w_P_old;
w_R_cur = w_R_old * relative_q;
double shift_yaw;
Matrix3d shift_r;
Vector3d shift_t;
if(use_imu)
{
shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();
shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));
}
else
shift_r = w_R_cur * vio_R_cur.transpose();
shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur;
// shift vio pose of whole sequence to the world frame
if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
{
w_r_vio = shift_r;
w_t_vio = shift_t;
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
list<KeyFrame*>::iterator it = keyframelist.begin();
for (; it != keyframelist.end(); it++)
{
if((*it)->sequence == cur_kf->sequence)
{
Vector3d vio_P_cur;
Matrix3d vio_R_cur;
(*it)->getVioPose(vio_P_cur, vio_R_cur);
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
(*it)->updateVioPose(vio_P_cur, vio_R_cur);
}
}
sequence_loop[cur_kf->sequence] = 1;
}
m_optimize_buf.lock();
optimize_buf.push(cur_kf->index);
m_optimize_buf.unlock();
}
}
其中shift_r和t表示當找到不同sequence之間的回環時，求得的兩個sequence之間的tf，之後會把所有新增的KeyFrame的vio位姿轉換到已有的sequence坐标下，儲存在上文提到的 vio_T_w_i，vio_R_w_i中，于是KeyFrame中的兩個vio pose就不一樣了，一個是前端發過來的pose，一個是将前端發過來的pose轉換到已有sequence坐标系（map坐标系，因為vins中暫時預設已有的sequence就是已經建立好的地圖，該sequence坐标系就是map坐标系）下的pose。，如果不提前加載地圖，那麼所有的KeyFrame應該位于同一sequence，w_r_vio就是個機關矩陣，不起任何作用，KeyFrame中的兩個vio pose就是相等的。
有個了這個了解之後，其他的算法才能了解清楚，還有個關鍵變量是r_drift，比較容易了解，看看optimize4DOF()。
...
Vector3d cur_t, vio_t;
Matrix3d cur_r, vio_r;
cur_kf->getPose(cur_t, cur_r);
cur_kf->getVioPose(vio_t, vio_r);
m_drift.lock();
yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();
r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));
t_drift = cur_t - r_drift * vio_t;
...
在優化完成之後，cur_kf的全局pose，也就是上文中的第二個pose被更新了，此時vio pose跟全局pose不相等了，r_drift就是兩者的內插補點，之後進來的KeyFrame都通過這個內插補點從vio pose中推導出全局pose，直到全局pose再被優化函數更新。
再來看看代碼整體邏輯和流程，主邏輯位于addKeyFrame()這個函數中:
如果不做回環這個後端基本就沒做啥了，detectLoop就是用DBOW在找回環，代碼邏輯很簡單就不詳解了，detectLoop如果通過DBOW找到了回環，會用回環找到的index索引到老的KeyFrame，并給到findConnection，由它去做進一步确認以及計算相對位姿，findConnection()也是一個很核心的函數，基本上論文中關于回環的部分都在這裡實作的，先看下主體邏輯：
核心就是用老的KeyFrame與目前KeyFrame進行比對得到結果，主要有以下幾個主要流程，論文中也都講過了：
- matchByBriefDes()，通過feature描述子比對，對于論文中VII-B中2D-2D比對，這裡提一句，在建構KeyFrame時，會提取所有feature的描述子并儲存在window_brief_descriptors，此外，額外在全圖範圍内提取了大量feature，并提取了這些feature的描述子并儲存在brief_descriptors，這裡的matchByBriefDes()就是用目前KeyFrame的window_brief_descriptors與老的KeyFrame的brief_descriptors一一計算HammingDistance，大于一定門檻值則認為比對上了，比對上的feature數量大于一定門檻值（預設值25）才進入下一步。
- PnPRANSAC()，對應論文中VII-B中3D-2D比對，名字很直覺了，通過上一步得到的比對到的feature對，進行PnPRANSAC操作，用以剔除掉錯誤的比對對，并計算出relative pose，同樣的，經過這一步後剩餘的比對feature對數量大于一定門檻值（預設值25）才進入下一步。
- 最後判斷上一步得到的relative pose的yaw和t滿足一定門檻值（預設30度和20米以内），則認為得到回環。
之後就是把回環因子加入優化問題使用ceres進行優化的東西了，沒有太多可以講了。