【SLAM】VINS-Fusion解析——流程

217
0
2020年10月20日 09时51分

VINS-Fusion分析

因为时间原因,没有像vins-mono看的和写的那么具体。有时间的话我会补充完整版。
vins-fusion不像mono那样有三个node,它只有一个node,在rosNodeTest.cpp里。我推测之所以这样做,是为了方便非ros版的c++项目使用。先上流程图。

在这里插入图片描述

 

vins-fusion和vins-mono关于单目和双目的区别,主要体现在特征点的三角化,初始化和重投影误差上。

 

mono的三角化,是对前一帧和后一帧进行的,
fusion的三角化,是对左目和双目进行的。
mono的初始化,非常复杂,先SfM,再与IMU进行松耦合相互标定;
fusion的初始化,是在三角化后,再进行一次非线性优化就完成了。
mono的重投影误差,是左目的i,j时刻关于共视点的重投影;
fusion的重投影误差,是左目的i时刻和右目的j时候关于共视点的重投影。

 

基本上所有的内容都在estimator.cpp。
几个比较印象深刻的的地方。

 

feature_tracker里面,

 

并没有对imageConstPtr的时间/频率校准工作,而只是放在img_buf里面;
单目光流跟踪加上了反向光流追踪。

 

数据预处理,

 

没有采用messurements这个大的数据结构,而是用了几个小的vec来存放着对齐的数据;
对于边界位置的IMU采用,放弃了被相邻两帧共享的线性插值处理;

 

processMeasurements:

 

对于状态量Rs,mono里面初始化是Identity,而在fusion里面,初始化是用acc的平均值与gw对比就获得了相对于先验w系的旋转,通过这个获得Rs[0];

 

然后初始化区别非常大,

 

没有construct和IMUvisual align那一些复杂的操作。此时PVQ都是w系的,那么左右双目的位姿也都是w系的,可以直接三角化双目的特征点获得特征点在w系上的坐标和逆深度;
之后就可以通过PnP进一步优化Ps和Rs。
最后非线性优化一下,就结束了。

 

TODO 而mono里面,能不能也采用一样的方式呢?一开始PVQ都是w系的,scaler也是,然后三角化不同pose,得到一些3D点,然后PnP优化?这样是不是精度就不行了?这是不是fusion双目版比mono版精度低的理由?

 

optimization:

 

重投影误差用的是不同camera之间的。

  //左相机在i时刻和j时刻分别观测到路标点
    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]);
}

if(STEREO && it_per_frame.is_stereo)
{                
    Vector3d pts_j_right = it_per_frame.pointRight;
    if(imu_i != imu_j)
    {   //左相机在i时刻、右相机在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
    {   //左相机和右相机在i时刻分别观测到路标点
        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]);
    }
   
}


发表评论

后才能评论