VINS_FUSION入门系列－优化optimization

最新推荐文章于 2024-07-13 16:16:21 发布

pj_find

最新推荐文章于 2024-07-13 16:16:21 发布

阅读量1.7k

点赞数 6

文章标签：自动驾驶

本文链接：https://blog.csdn.net/pj_find/article/details/106497124

版权

本文介绍了VINS_FUSION的优化核心，包括视觉约束的三个部分：ProjectionOneFrameTwoCamFactor、ProjectionTwoFrameOneCamFactor、ProjectionTwoFrameTwoCamFactor，以及IMU预积分和边缘化策略。通过残差计算和雅克比矩阵分析，展示了VINS_FUSION如何融合双目视觉与IMU数据进行状态估计。

摘要由CSDN通过智能技术生成

代码最核心的地方就是优化和滑动窗口,代码主要和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