转载自:https://blog.csdn.net/hltt3838/article/details/109514591
备用查看
理论部分
对陀螺仪bias的校正
代码部分
if(STEREO && USE_IMU)
{
//有了深度之后就可以进行PnP求解
f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
// 双目三角化
f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
if (frame_count == WINDOW_SIZE)
{
map<double, ImageFrame>::iterator frame_it;
int i = 0;
for (frame_it = all_image_frame.begin(); frame_it != all_image_frame.end(); frame_it++)
{
frame_it->second.R = Rs[i];
frame_it->second.T = Ps[i];
i++;
}
solveGyroscopeBias(all_image_frame, Bgs);
for (int i = 0; i <= WINDOW_SIZE; i++)
{
pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
}
optimization();
updateLatestStates(); //让此时刻的值都等于上一时刻的值,用来更新状态
solver_flag = NON_LINEAR;
slideWindow();
ROS_INFO("Initialization finish!");
}
}