參考資料:
[1]VINS-Mono源碼解析（五）后端: 緊耦合優(yōu)化https://blog.csdn.net/q597967420/article/details/76099443
[2]http://zhehangt.win/2018/04/24/SLAM/VINS/VINSVIO/
[3]https://www.zhihu.com/question/63754583/answer/259699612
[4]中文注釋:https://github.com/castiel520/VINS-Mono/blob/master/vins_estimator/src/estimator.cpp
整體思路:
Step1:添加待優(yōu)化的狀態(tài)量
1.1添加p,q,speed,ba,bg
1.2添加相機(jī)和IMU外參p_cb,q_cb
1.3將優(yōu)化變量存入數(shù)組.因?yàn)閏eres用的是double類型的數(shù)組,所以要做vector到double類型的變換 < WINDOW_SIZE - 2))
Step2:添加殘差
2.1添加邊緣化殘差
2.2添加IMU殘差.滑動(dòng)窗口中的相鄰兩幀之間都有一個(gè)IMU殘差.滑動(dòng)窗口的大小是10.共有10個(gè)IMU殘差項(xiàng).注意:這里的IMU項(xiàng)和camera項(xiàng)之間是有一個(gè)系數(shù)的,這個(gè)系數(shù)就是他們各自的協(xié)方差矩陣,IMU的協(xié)方差就是預(yù)計(jì)分的協(xié)方差,視覺(jué)的協(xié)方差就是一個(gè)固定的系數(shù),f/1.5.(1.5是特征點(diǎn)追蹤的方差)
2.3添加視覺(jué)殘差.針對(duì)滑動(dòng)窗口中的所有特征點(diǎn).只要該特征點(diǎn)被觀測(cè)的次數(shù)大于2次并且觀測(cè)到該特征點(diǎn)的首幀在滑動(dòng)窗口的前7才行.然后通過(guò)觀測(cè)該特征點(diǎn)的兩幀建立殘差.
這里忽略閉環(huán)校正的情況
2.4然后設(shè)置求解器屬性,進(jìn)行求解問(wèn)題.這里設(shè)置的最大迭代次數(shù)是8,最大求解時(shí)間是0.04ms,為了保證實(shí)時(shí).
Step3:marg部分
3.1對(duì)于邊緣化首幀
3.1.1把之前存的殘差部分加進(jìn)來(lái)
3.1.2把與首幀相關(guān)的殘差項(xiàng)加進(jìn)來(lái),包含IMU,vision.
3.1.3計(jì)算所有殘差項(xiàng)的殘差和雅克比
3.1.4多線程構(gòu)造Hx=b的結(jié)構(gòu),(需要細(xì)看)
3.1.5marg結(jié)束,調(diào)整參數(shù)塊在下一次window的位置
3.2對(duì)于邊緣化倒數(shù)第二幀
3.2.1如果倒數(shù)第二幀不是關(guān)鍵幀,保留該幀的IMU測(cè)量,去掉該幀的visual,代碼中都沒(méi)寫.
3.2.2計(jì)算所有殘差項(xiàng)的殘差和雅克比
3.2.3多線程構(gòu)造Hx=b的結(jié)構(gòu),(需要細(xì)看)
3.2.4marg結(jié)束,調(diào)整參數(shù)塊在下一次window的位置
相關(guān)代碼:
void Estimator::optimization()
{
ceres::Problem problem;
ceres::LossFunction *loss_function;
//loss_function = new ceres::HuberLoss(1.0);
//！設(shè)置柯西損失函數(shù)因子
loss_function = new ceres::CauchyLoss(1.0);
//！Step1:添加待優(yōu)化狀態(tài)量
//！Step1.1：添加[p,q](7)，[speed,ba,bg](9)
for (int i = 0; i < WINDOW_SIZE + 1; i++)
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
}
//！Step1.2：添加相機(jī)與IMU的外參[p_cb,q_cb](7)
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)
{
ROS_DEBUG("fix extinsic param");
problem.SetParameterBlockConstant(para_Ex_Pose[i]);
}
else
ROS_DEBUG("estimate extinsic param");
}
//！將優(yōu)化量存入數(shù)組
TicToc t_whole, t_prepare;
// 因?yàn)閏eres用的是double數(shù)組,所以下面用vector2double做類型轉(zhuǎn)換
vector2double();
//！Step2.1:添加邊緣化的殘差
// last_marginalization_parameter_blocks指的就是和被邊緣化的變量有約束關(guān)系的變量,也就是heyijia博客中的Xb.
// 這個(gè)marginalization的結(jié)構(gòu)是始終存在的,隨著marg結(jié)構(gòu)的不斷更新,last_marginalization_parameter_blocks對(duì)應(yīng)的還是滑動(dòng)窗口中的變量
// last_marginalization_info 就是Xb對(duì)應(yīng)的測(cè)量Zb,將這個(gè)約束作為Xb的先驗(yàn),
if (last_marginalization_info)
{
// construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
problem.AddResidualBlock(marginalization_factor, NULL,
last_marginalization_parameter_blocks);
}
//！Step2.2:添加IMU的residual
// 這里IMU項(xiàng)和camera項(xiàng)之間是有一個(gè)系數(shù)的,這個(gè)系數(shù)就是他們各自的協(xié)方差矩陣,IMU的協(xié)方差就是預(yù)積分的協(xié)方差(IMUFacotor::Evaluate,中添加IMU協(xié)方差,求解jacibian矩陣),
// 而camera的測(cè)量殘差則是一個(gè)固定的系數(shù).
for (int i = 0; i < WINDOW_SIZE; i++)
{
int j = i + 1;
if (pre_integrations[j]->sum_dt > 10.0)
continue;
//！添加代價(jià)函數(shù)
IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
//！注意在添加殘差的組成部分，由前后兩幀的[p,q,v,b]組成，在計(jì)算雅克比的時(shí)候[p,q](7),[v,b](9)分開計(jì)算
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
}
//！Step2.3:添加視覺(jué)的residual
int f_m_cnt = 0;
int feature_index = -1;
//feature是滑動(dòng)窗口內(nèi)所有的特征點(diǎn)的集合
for (auto &it_per_id : f_manager.feature)
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
// 如果這個(gè)特征點(diǎn)被觀測(cè)的次數(shù)大于等于2 并且首次觀測(cè)到該特征點(diǎn)的幀小于滑動(dòng)窗口倒數(shù)第3幀,這個(gè)特征點(diǎn)就可以建立一個(gè)殘差
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
++feature_index;
//！得到觀測(cè)到該特征點(diǎn)的首幀
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
//！得到首幀觀測(cè)到的特征點(diǎn)的歸一化相機(jī)坐標(biāo)
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)
{
continue;
}
//！得到第二個(gè)特征點(diǎn)
Vector3d pts_j = it_per_frame.point;
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
f_m_cnt++;
}
}
relocalize = false;
//！Step2.3:添加視覺(jué)的residual
//！添加閉環(huán)校正時(shí)候的狀態(tài)量和殘差
//loop close factor
if(LOOP_CLOSURE)
{
int loop_constraint_num = 0;
//！遍歷閉環(huán)檢測(cè) database
for (int k = 0; k < (int)retrive_data_vector.size(); k++)
{
//！遍歷滑窗內(nèi)的Keyframe
for(int i = 0; i < WINDOW_SIZE; i++)
{
//！為什么這樣就閉環(huán)成功了呢？這個(gè)地方通過(guò)時(shí)間戳就閉環(huán)了？
//！建立閉環(huán)約束
if(retrive_data_vector[k].header == Headers[i].stamp.toSec())
{
relocalize = true;
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(retrive_data_vector[k].loop_pose, SIZE_POSE, local_parameterization);
loop_window_index = i;
loop_constraint_num++;
int retrive_feature_index = 0;
int feature_index = -1;
//！遍歷滑窗內(nèi)的特征點(diǎn)
for (auto &it_per_id : f_manager.feature)
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
//！至少有兩幀圖像觀測(cè)到該特征點(diǎn)且不是滑窗內(nèi)的最后兩幀
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
++feature_index;
int start = it_per_id.start_frame;
//！如果該Feature是被滑窗中本幀之前觀測(cè)到
if(start <= i)
{
while(retrive_data_vector[k].features_ids[retrive_feature_index] < it_per_id.feature_id)
{
retrive_feature_index++;
}
//！將擁有固定位姿的閉環(huán)幀加入到Visual-Inertail BA中
if(retrive_data_vector[k].features_ids[retrive_feature_index] == it_per_id.feature_id)
{
Vector3d pts_j = Vector3d(retrive_data_vector[k].measurements[retrive_feature_index].x, retrive_data_vector[k].measurements[retrive_feature_index].y, 1.0);
Vector3d pts_i = it_per_id.feature_per_frame[0].point;
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[start], retrive_data_vector[k].loop_pose, para_Ex_Pose[0], para_Feature[feature_index]);
retrive_feature_index++;
}
}
}
}
}
}
ROS_DEBUG("loop constraint num: %d", loop_constraint_num);
}
ROS_DEBUG("visual measurement count: %d", f_m_cnt);
ROS_DEBUG("prepare for ceres: %f", t_prepare.toc());
//！設(shè)置求解器的屬性
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;//8
//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;//0.04ms
//！求解problem
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()));
ROS_DEBUG("solver costs: %f", t_solver.toc());
/*****************優(yōu)化后的內(nèi)容********************/
//！求解兩個(gè)閉環(huán)幀之間的關(guān)系
// relative info between two loop frame
if(LOOP_CLOSURE && relocalize)
{
for (int k = 0; k < (int)retrive_data_vector.size(); k++)
{
for(int i = 0; i< WINDOW_SIZE; i++)
{
//！閉環(huán)檢測(cè)成功
if(retrive_data_vector[k].header == Headers[i].stamp.toSec())
{
retrive_data_vector[k].relative_pose = true;
Matrix3d Rs_i = Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]).normalized().toRotationMatrix();
Vector3d Ps_i = Vector3d(para_Pose[i][0], para_Pose[i][1], para_Pose[i][2]);
//！閉環(huán)幀的位姿
Quaterniond Qs_loop;
Qs_loop = Quaterniond(retrive_data_vector[k].loop_pose[6], retrive_data_vector[k].loop_pose[3], retrive_data_vector[k].loop_pose[4], retrive_data_vector[k].loop_pose[5]).normalized().toRotationMatrix();
Matrix3d Rs_loop = Qs_loop.toRotationMatrix();
Vector3d Ps_loop = Vector3d( retrive_data_vector[k].loop_pose[0], retrive_data_vector[k].loop_pose[1], retrive_data_vector[k].loop_pose[2]);
//！求匹配幀到閉環(huán)幀之間的相對(duì)位姿
retrive_data_vector[k].relative_t = Rs_loop.transpose() * (Ps_i - Ps_loop);
retrive_data_vector[k].relative_q = Rs_loop.transpose() * Rs_i;
//！因?yàn)閜itch和roll可觀，所以僅考慮在yaw上的漂移
retrive_data_vector[k].relative_yaw = Utility::normalizeAngle(Utility::R2ypr(Rs_i).x() - Utility::R2ypr(Rs_loop).x());
//！
if (abs(retrive_data_vector[k].relative_yaw) > 30.0 || retrive_data_vector[k].relative_t.norm() > 20.0)
retrive_data_vector[k].relative_pose = false;
}
}
}
}
double2vector();
//！Step3：marg部分
//1.把之前存的殘差部分加進(jìn)來(lái)
//2.把與當(dāng)前要marg掉的幀的所有相關(guān)殘差項(xiàng)加進(jìn)來(lái),IMU,vision.
//3.preMarginalize-> 調(diào)用Evaluate計(jì)算所有ResidualBlock的殘差和雅克比,parameter_block_data是margniliazation中存參數(shù)塊的容器
//4.多線程構(gòu)造Hx=b的結(jié)構(gòu),H是邊緣化后的結(jié)果,First Estimate Jacobian,在X0處進(jìn)行線性化,需要去看!!!!???????????????????????????
//5.marg結(jié)束,調(diào)整參數(shù)塊在下一次window中對(duì)應(yīng)的位置
TicToc t_whole_marginalization;
if (marginalization_flag == MARGIN_OLD)
{
MarginalizationInfo *marginalization_info = new MarginalizationInfo();
vector2double();
//! 先驗(yàn)誤差會(huì)一直保存，而不是只使用一次
//! 如果上一次邊緣化的信息存在
//! 要邊緣化的參數(shù)塊是 para_Pose[0] para_SpeedBias[0] 以及 para_Feature[feature_index](滑窗內(nèi)的第feature_index個(gè)點(diǎn)的逆深度)
if (last_marginalization_info)
{
vector<int> drop_set;
for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
{
//！查詢last_marginalization_parameter_blocks中是首幀狀態(tài)量的序號(hào)
if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||
last_marginalization_parameter_blocks[i] == para_SpeedBias[0])
drop_set.push_back(i);
}
//! 構(gòu)造邊緣化的的Factor
// construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
//! 添加上一次邊緣化的參數(shù)塊
//！cost_function, loss_function, 待估計(jì)參數(shù)(last_marginalization_parameter_blocks, drop_set)
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
last_marginalization_parameter_blocks,
drop_set);
marginalization_info->addResidualBlockInfo(residual_block_info);
}
//！添加IMU的先驗(yàn)，只包含邊緣化幀的IMU測(cè)量殘差
//！Question：不應(yīng)該是pre_integrations[0]么
//!
{
if (pre_integrations[1]->sum_dt < 10.0)
{
IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
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});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
//！添加視覺(jué)的先驗(yàn)，只添加起始幀是舊幀且觀測(cè)次數(shù)大于2的Features
{
int feature_index = -1;
//! 遍歷滑窗內(nèi)所有的Features
for (auto &it_per_id : f_manager.feature)
{
//! 該特征點(diǎn)被觀測(cè)到的次數(shù)
it_per_id.used_num = it_per_id.feature_per_frame.size();
//! Feature的觀測(cè)次數(shù)不小于2次，且起始幀不屬于最后兩幀
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
++feature_index;
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
//! 只選擇被邊緣化的幀的Features
if (imu_i != 0)
continue;
//! 得到該Feature在起始下的歸一化坐標(biāo)
Vector3d pts_i = it_per_id.feature_per_frame[0].point;
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
//! 不需要起始觀測(cè)幀
if (imu_i == imu_j)
continue;
Vector3d pts_j = it_per_frame.point;
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,
vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},
vector<int>{0, 3});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
}
//! 將三個(gè)ResidualBlockInfo中的參數(shù)塊綜合到marginalization_info中
// 計(jì)算所有ResidualBlock(殘差項(xiàng))的殘差和雅克比,parameter_block_data是參數(shù)塊的容器
TicToc t_pre_margin;
marginalization_info->preMarginalize();
ROS_DEBUG("pre marginalization %f ms", t_pre_margin.toc());
//!
TicToc t_margin;
marginalization_info->marginalize();
ROS_DEBUG("marginalization %f ms", t_margin.toc());
//！將滑窗里關(guān)鍵幀位姿移位，為什么是向右移位了呢？
//! 這里是保存了所有狀態(tài)量的信息，為什么沒(méi)有保存逆深度的狀態(tài)量呢
std::unordered_map<long, double *> addr_shift;
for (int i = 1; i <= WINDOW_SIZE; i++)
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
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];
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;
}
//！邊緣化倒數(shù)第二幀
//如果倒數(shù)第二幀不是關(guān)鍵幀
//1.保留該幀的IMU測(cè)量,去掉該幀的visual,代碼中都沒(méi)有寫.
//2.premarg
//3.marg
//4.滑動(dòng)窗口移動(dòng)
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)
{
vector<int> drop_set;
for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
{
//！尋找導(dǎo)數(shù)第二幀的位姿
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];
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
}
else
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];
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];
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;
}
}
ROS_DEBUG("whole marginalization costs: %f", t_whole_marginalization.toc());
ROS_DEBUG("whole time for ceres: %f", t_whole.toc());
}
