代码最核心的地方就是优化和滑动窗口,代码主要和VINS_MONO分成的三部分差不多,视觉由于使用的双目有所改变.优化主要就是计算残差,雅克比矩阵和信息矩阵.
VINS_MONO
VINS_FUSION
从上图可以看出,总体VINS_MONO和VINS_FUSION求解的变量差不多.下面具体分析VINS_FUSION的优化.
1.1 视觉约束
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] */
}
// 如果是双目的
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.velocityR