0 前情回顧
VINS-FUSION代碼超詳細註釋（VIO部分）/VIO入門(1)講到了主程序rosNodeTest.cpp。在程序最後，會進入sync_process線程進行處理。本篇博客接着進行講解。
VINS-FUSION代碼超詳細註釋（VIO部分）/VIO入門(2)中,講了sync_process,以及其中的trackImage和processMeasurements,包括processMeasurements中對IMU數據的處理部分.
VINS-FUSION代碼超詳細註釋（VIO部分）/VIO入門(3)中,講了processImage,其中就包含了初始化,三角化,非線性優化,劃窗等等內容.
本文主要對函數optimization() updateLatestStates() slideWindow()進行介紹.
本次工作
我首先一步步的把代碼全部註釋了，十分的詳細，對於C++和OpenCV的一些操作也進行了詳細的註釋，對於剛入門的同學應該還是有幫助的。之後我將代碼開源，並寫了相應的博客進行講解。
開源程序:
https://github.com/kuankuan-yue/VINS-FUSION-leanrning.git
相應博客：
VINS-FUSION代碼超詳細註釋（VIO部分）/VIO入門(1)
VINS-FUSION代碼超詳細註釋（VIO部分）/VIO入門(2)
VINS-FUSION代碼超詳細註釋（VIO部分）/VIO入門(3)
VINS-FUSION代碼超詳細註釋（VIO部分）/VIO入門(4)
1 updateLatestStates() 和slideWindow()
這兩個函數比較簡單
// 讓此時刻的值都等於上一時刻的值,用來更新狀態
void Estimator::updateLatestStates()
// 滑動窗口法
void Estimator::slideWindow()
// 道理很簡單,就是把前後元素交換
2 optimization()
這個函數可以說是整個VIO的精華和難點所在!
因爲內容太多了,所以直接貼了代碼.對於其中某些函數,如果大家有什麼問題的話,可以去github上參考我的代碼.
// 基於滑動窗口的緊耦合的非線性優化，殘差項的構造和求解
void Estimator::optimization()
{
TicToc t_whole, t_prepare;
vector2double();
//------------------ 定義問題 定義本地參數化,並添加優化參數-------------------------------------------------
ceres::Problem problem;// 定義ceres的優化問題
ceres::LossFunction *loss_function;//核函數
//loss_function = NULL;
loss_function = new ceres::HuberLoss(1.0);//HuberLoss當預測偏差小於 δ 時，它採用平方誤差,當預測偏差大於 δ 時，採用的線性誤差。
//loss_function = new ceres::CauchyLoss(1.0 / FOCAL_LENGTH);
//ceres::LossFunction* loss_function = new ceres::HuberLoss(1.0);
for (int i = 0; i < frame_count + 1; i++)
{
// 對於四元數或者旋轉矩陣這種使用過參數化表示旋轉的方式，它們是不支持廣義的加法
// 所以我們在使用ceres對其進行迭代更新的時候就需要自定義其更新方式了，具體的做法是實現一個LocalParameterization
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
// AddParameterBlock 向該問題添加具有適當大小和參數化的參數塊。
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization); //因爲有四元數,所以使用了 local_parameterization
if(USE_IMU)
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);//使用默認的加法
}
// 沒使用imu時,將窗口內第一幀的位姿固定
if(!USE_IMU)
// SetParameterBlockConstant 在優化過程中，使指示的參數塊保持恆定。設置任何參數塊變成一個常量
// 固定第一幀的位姿不變! 這裏涉及到論文2中的
problem.SetParameterBlockConstant(para_Pose[0]);
for (int i = 0; i < NUM_OF_CAM; i++)
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);//如果是雙目,估計兩個相機的位姿
if ((ESTIMATE_EXTRINSIC && frame_count == WINDOW_SIZE && Vs[0].norm() > 0.2) || openExEstimation)
//Vs[0].norm() > 0.2窗口內第一個速度>2?
{
//ROS_INFO("estimate extinsic param");
openExEstimation = 1;//打開外部估計
}
else//如果不需要估計,則把估計器中的外部參數設爲定值
{
//ROS_INFO("fix extinsic param");
problem.SetParameterBlockConstant(para_Ex_Pose[i]);
}
}
problem.AddParameterBlock(para_Td[0], 1);//把時間也作爲待優化變量
if (!ESTIMATE_TD || Vs[0].norm() < 0.2)//如果不估計時間就固定
problem.SetParameterBlockConstant(para_Td[0]);
// ------------------------在問題中添加約束,也就是構造殘差函數----------------------------------
// 在問題中添加先驗信息作爲約束
if (last_marginalization_info && last_marginalization_info->valid)
{
// 構造新的marginisation_factor construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
/* 通過提供參數塊的向量來添加殘差塊。
ResidualBlockId AddResidualBlock(
CostFunction* cost_function,//損失函數
LossFunction* loss_function,//核函數
const std::vector<double*>& parameter_blocks); */
problem.AddResidualBlock(marginalization_factor, NULL,
last_marginalization_parameter_blocks);
}
// 在問題中添加IMU約束
if(USE_IMU)
{
for (int i = 0; i < frame_count; i++)
{
int j = i + 1;
if (pre_integrations[j]->sum_dt > 10.0)
continue;
IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
//這裏添加的參數包括狀態i和狀態j
}
}
int f_m_cnt = 0; //每個特徵點,觀測到它的相機的計數 visual measurement count
int feature_index = -1;
for (auto &it_per_id : f_manager.feature)
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
if (it_per_id.used_num < 4)
continue;
++feature_index;
// imu_i該特徵點第一次被觀測到的幀 ,imu_j = imu_i - 1
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
Vector3d pts_i = it_per_id.feature_per_frame[0].point;
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
if (imu_i != imu_j)//既,本次不是第一次觀測到
{
Vector3d pts_j = it_per_frame.point;
ProjectionTwoFrameOneCamFactor *f_td = new ProjectionTwoFrameOneCamFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);
/* 相關介紹:
1 只在視覺量測中用了核函數loss_function 用的是huber
2 參數包含了para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]
3 ProjectionTwoFrameOneCamFactor這個重投影並不是很懂 */
}
// 如果是雙目的
if(STEREO && it_per_frame.is_stereo)
{
Vector3d pts_j_right = it_per_frame.pointRight;
if(imu_i != imu_j) //既,本次不是第一次觀測到
{
ProjectionTwoFrameTwoCamFactor *f = new ProjectionTwoFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,
it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]);
}
else//既,本次是第一次觀測到
{
ProjectionOneFrameTwoCamFactor *f = new ProjectionOneFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,
it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
problem.AddResidualBlock(f, loss_function, para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]);
}
}
f_m_cnt++;
}
}
ROS_DEBUG("visual measurement count: %d", f_m_cnt);
//printf("prepare for ceres: %f \n", t_prepare.toc());
// ------------------------------------寫下來配置優化選項,並進行求解-----------------------------------------
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.num_threads = 2;
options.trust_region_strategy_type = ceres::DOGLEG;
options.max_num_iterations = NUM_ITERATIONS;
//options.use_explicit_schur_complement = true;
//options.minimizer_progress_to_stdout = true;
//options.use_nonmonotonic_steps = true;
if (marginalization_flag == MARGIN_OLD)
options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;
else
options.max_solver_time_in_seconds = SOLVER_TIME;
TicToc t_solver;
ceres::Solver::Summary summary;//優化信息
ceres::Solve(options, &problem, &summary);
//cout << summary.BriefReport() << endl;
ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));
//printf("solver costs: %f \n", t_solver.toc());
double2vector();
//printf("frame_count: %d \n", frame_count);
if(frame_count < WINDOW_SIZE)
return;
// -----------------------------marginalization------------------------------------
TicToc t_whole_marginalization;
//如果需要marg掉最老的一幀
if (marginalization_flag == MARGIN_OLD)
{
MarginalizationInfo *marginalization_info = new MarginalizationInfo();//先驗信息
vector2double();
if (last_marginalization_info && last_marginalization_info->valid)
{
vector<int> drop_set;//邊緣話的優化變量的位置_drop_set
for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
//last_marginalization_parameter_blocks 是上一輪留下來的殘差塊
{
if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||
last_marginalization_parameter_blocks[i] == para_SpeedBias[0])
//需要marg掉的優化變量，也就是滑窗內第一個變量,para_Pose[0]和para_SpeedBias[0]
drop_set.push_back(i);
}
// 創建新的marg因子 construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
/* 是爲了將不同的損失函數_cost_function以及優化變量_parameter_blocks統一起來再一起添加到marginalization_info中
ResidualBlockInfo(ceres::CostFunction *_cost_function,
ceres::LossFunction *_loss_function,
std::vector<double *> _parameter_blocks,
std::vector<int> _drop_set) */
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
last_marginalization_parameter_blocks,
drop_set);//這一步添加了marg信息
// 將上一步marginalization後的信息作爲先驗信息
marginalization_info->addResidualBlockInfo(residual_block_info);
}
// 添加IMU的marg信息
// 然後添加第0幀和第1幀之間的IMU預積分值以及第0幀和第1幀相關優化變量
if(USE_IMU)
{
if (pre_integrations[1]->sum_dt < 10.0)
{
IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
// 這一步添加IMU的marg信息
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(imu_factor, NULL,
vector<double *>{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]},//優化變量
vector<int>{0, 1});//這裏是0,1的原因是0和1是para_Pose[0], para_SpeedBias[0]是需要marg的變量
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
// 添加視覺的maeg信息
{
int feature_index = -1;
//這裏是遍歷滑窗所有的特徵點
for (auto &it_per_id : f_manager.feature)
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
if (it_per_id.used_num < 4)
continue;
++feature_index;
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;//這裏是從特徵點的第一個觀察幀開始
if (imu_i != 0)//如果第一個觀察幀不是第一幀就不進行考慮，因此後面用來構建marg矩陣的都是和第一幀有共視關係的滑窗幀
continue;
Vector3d pts_i = it_per_id.feature_per_frame[0].point;
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
if(imu_i != imu_j)
{
Vector3d pts_j = it_per_frame.point;
ProjectionTwoFrameOneCamFactor *f_td = new ProjectionTwoFrameOneCamFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f_td, loss_function,
vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]},//優化變量
vector<int>{0, 3});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
if(STEREO && it_per_frame.is_stereo)
{
Vector3d pts_j_right = it_per_frame.pointRight;
if(imu_i != imu_j)
{
ProjectionTwoFrameTwoCamFactor *f = new ProjectionTwoFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,
it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,
vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]},//優化變量
vector<int>{0, 4});//爲0和3的原因是，para_Pose[imu_i]是第一幀的位姿，需要marg掉，而3是para_Feature[feature_index]是和第一幀相關的特徵點，需要marg掉
marginalization_info->addResidualBlockInfo(residual_block_info);
}
else
{
ProjectionOneFrameTwoCamFactor *f = new ProjectionOneFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,
it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,
vector<double *>{para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]},
vector<int>{2});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
}
}
}
TicToc t_pre_margin;
// 上面通過調用 addResidualBlockInfo() 已經確定優化變量的數量、存儲位置、長度以及待優化變量的數量以及存儲位置，
//-------------------------- 下面就需要調用 preMarginalize() 進行預處理
marginalization_info->preMarginalize();
ROS_DEBUG("pre marginalization %f ms", t_pre_margin.toc());
//------------------------調用 marginalize 正式開始邊緣化
TicToc t_margin;
marginalization_info->marginalize();
ROS_DEBUG("marginalization %f ms", t_margin.toc());
//------------------------在optimization的最後會有一部滑窗預移動的操作
// 值得注意的是，這裏僅僅是相當於將指針進行了一次移動，指針對應的數據還是舊數據，因此需要結合後面調用的 slideWindow() 函數才能實現真正的滑窗移動
std::unordered_map<long, double *> addr_shift;
for (int i = 1; i <= WINDOW_SIZE; i++)//從1開始，因爲第一幀的狀態不要了
{
//這一步的操作指的是第i的位置存放的的是i-1的內容，這就意味着窗口向前移動了一格
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];//因此para_Pose這些變量都是雙指針變量，因此這一步是指針操作
if(USE_IMU)
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
}
for (int i = 0; i < NUM_OF_CAM; i++)
addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
if (last_marginalization_info)
delete last_marginalization_info;//刪除掉上一次的marg相關的內容
last_marginalization_info = marginalization_info;//marg相關內容的遞歸
last_marginalization_parameter_blocks = parameter_blocks;//優化變量的遞歸，這裏面僅僅是指針
}
else
{
if (last_marginalization_info &&
std::count(std::begin(last_marginalization_parameter_blocks), std::end(last_marginalization_parameter_blocks), para_Pose[WINDOW_SIZE - 1]))
{
MarginalizationInfo *marginalization_info = new MarginalizationInfo();
vector2double();
if (last_marginalization_info && last_marginalization_info->valid)
{
vector<int> drop_set;
for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
{
ROS_ASSERT(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1]);
if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1])
drop_set.push_back(i);
}
// construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
last_marginalization_parameter_blocks,
drop_set);
marginalization_info->addResidualBlockInfo(residual_block_info);
}
TicToc t_pre_margin;
ROS_DEBUG("begin marginalization");
marginalization_info->preMarginalize();
ROS_DEBUG("end pre marginalization, %f ms", t_pre_margin.toc());
TicToc t_margin;
ROS_DEBUG("begin marginalization");
marginalization_info->marginalize();
ROS_DEBUG("end marginalization, %f ms", t_margin.toc());
std::unordered_map<long, double *> addr_shift;
for (int i = 0; i <= WINDOW_SIZE; i++)
{
if (i == WINDOW_SIZE - 1)
continue;
else if (i == WINDOW_SIZE)
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
if(USE_IMU)
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
}
else
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];
if(USE_IMU)
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i];
}
}
for (int i = 0; i < NUM_OF_CAM; i++)
addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
if (last_marginalization_info)
delete last_marginalization_info;
last_marginalization_info = marginalization_info;
last_marginalization_parameter_blocks = parameter_blocks;
}
}
//printf("whole marginalization costs: %f \n", t_whole_marginalization.toc());
//printf("whole time for ceres: %f \n", t_whole.toc());
}