VINS-FUSION代码超详细注释(VIO部分)/VIO入门(1)讲到了主程序rosNodeTest.cpp。在程序最后,会进入sync_process线程进行处理。本篇博客接着进行讲解。
VINS-FUSION代码超详细注释(VIO部分)/VIO入门(2)中,讲了sync_process
,以及其中的trackImage
和processMeasurements
,包括processMeasurements
中对IMU数据的处理部分.
本文开始说明图片处理的部分
我首先一步步的把代码全部注释了,十分的详细,对于C++和OpenCV的一些操作也进行了详细的注释,对于刚入门的同学应该还是有帮助的。之后我将代码开源,并写了相应的博客进行讲解。
开源程序:
https://github.com/kuankuan-yue/VINS-FUSION-leanrning.git
相应博客:
VINS-FUSION代码超详细注释(VIO部分)/VIO入门(1)
VINS-FUSION代码超详细注释(VIO部分)/VIO入门(2)
VINS-FUSION代码超详细注释(VIO部分)/VIO入门(3)
VINS-FUSION代码超详细注释(VIO部分)/VIO入门(4)
函数入口processMeasurements
中的processImage(feature.second, feature.first);
输入的之后关键帧和时间
/* addFeatureCheckParallax
对当前帧与之前帧进行视差比较,如果是当前帧变化很小,就会删去倒数第二帧,如果变化很大,就删去最旧的帧。并把这一帧作为新的关键帧
这样也就保证了划窗内优化的,除了最后一帧可能不是关键帧外,其余的都是关键帧
VINS里为了控制优化计算量,在实时情况下,只对当前帧之前某一部分帧进行优化,而不是全部历史帧。局部优化帧的数量就是窗口大小。
为了维持窗口大小,需要去除旧的帧添加新的帧,也就是边缘化 Marginalization。到底是删去最旧的帧(MARGIN_OLD)还是删去刚
刚进来窗口倒数第二帧(MARGIN_SECOND_NEW)
如果大于最小像素,则返回true */
bool FeatureManager::addFeatureCheckParallax
判断之后,确定marg掉那个帧
// 检测关键帧
if (f_manager.addFeatureCheckParallax(frame_count, image, td))
{
marginalization_flag = MARGIN_OLD;//新一阵将被作为关键帧!
//printf("keyframe\n");
}
else
{
marginalization_flag = MARGIN_SECOND_NEW;
//printf("non-keyframe\n");
}
// 估计一个外部参,并把ESTIMATE_EXTRINSIC置1,输出ric和RIC
if(ESTIMATE_EXTRINSIC == 2)
首先找到对应的图像
vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
// 这个里边放的是新图像和上一帧
使用CalibrationExRotation
计算参数
/* CalibrationExRotation
当外参完全不知道的时候,可以在线对其进行初步估计,然后在后续优化时,会在optimize函数中再次优化。
输入是新图像和上一阵图像的位姿 和二者之间的imu预积分值,输出旋转矩阵
对应VIO课程第七讲中对外参矩阵的求解 */
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
估计外部参数使用理论指导的,在VIO课程的第七讲中有介绍.
如果没有的话,进行初始化部分.
【泡泡读者来稿】VINS 论文推导及代码解析(二)
有对初始化的详细理论介绍
if (!STEREO && USE_IMU)
//有外参且当前帧时间戳大于初始化时间戳0.1秒,就进行初始化操作
if(ESTIMATE_EXTRINSIC != 2 && (header - initial_timestamp) > 0.1)//initial_timestamp设为了0
{
result = initialStructure(); //视觉惯性联合初始化
initial_timestamp = header; //更新初始化时间戳
}
其中的initialStructure
还含有很多函数.
// 视觉和惯性的对其,对应https://mp.weixin.qq.com/s/9twYJMOE8oydAzqND0UmFw中的visualInitialAlign
/* visualInitialAlign
很具VIO课程第七讲:一共分为5步:
1估计旋转外参. 2估计陀螺仪bias 3估计中立方向,速度.尺度初始值 4对重力加速度进一步优化 5将轨迹对其到世界坐标系 */
bool Estimator::visualInitialAlign()
// 视觉IMu的对其
bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)
// 更新得到新的陀螺仪漂移Bgs
// 对应视觉IMU对其的第二部分
// 对应https://mp.weixin.qq.com/s/9twYJMOE8oydAzqND0UmFw中的公式31-34
void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
// 初始化速度、重力和尺度因子
// 对应 https://mp.weixin.qq.com/s/9twYJMOE8oydAzqND0UmFw 中的公式34-36
bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
【泡泡读者来稿】VINS 论文推导及代码解析(二)中都有详细的介绍.可以参考.
成功初始化之后
其中的optimization() updateLatestStates() slideWindow()
在下一篇博客进行讲解
if(result)//如果初始化成功
{
optimization();//先进行一次滑动窗口非线性优化,得到当前帧与第一帧的位姿
updateLatestStates();
solver_flag = NON_LINEAR;
slideWindow();//滑动窗口
ROS_INFO("Initialization finish!");
}
else//滑掉这一窗
slideWindow();
// stereo + IMU initilization
if(STEREO && USE_IMU)
求解深度
// 双目三角化
// 结果放入了feature的estimated_depth中
void FeatureManager::triangulate(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])
// 利用svd方法对双目进行三角化
void FeatureManager::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d)
有了深度之后就可以进行PnP求解
// 有了深度,当下一帧照片来到以后就可以利用pnp求解位姿了
void FeatureManager::initFramePoseByPnP(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])
之后求解陀螺仪的偏执,并对IMU预积分值进行重新传播
solveGyroscopeBias(all_image_frame, Bgs);
// 对之前预积分得到的结果进行更新。
// 预积分的好处查看就在于你得到新的Bgs,不需要又重新再积分一遍,可以通过Bgs对位姿,速度的一阶导数,进行线性近似,得到新的Bgs求解出MU的最终结果。
for (int i = 0; i <= WINDOW_SIZE; i++)
{
pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
}
在至今进行优化
optimization();
updateLatestStates();
solver_flag = NON_LINEAR;
slideWindow();
其中的optimization() updateLatestStates() slideWindow()
在下一篇博客进行讲解
// stereo only initilization
if(STEREO && !USE_IMU)
{
f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
optimization();
if(frame_count == WINDOW_SIZE)
{
optimization();
updateLatestStates();
solver_flag = NON_LINEAR;
slideWindow();
ROS_INFO("Initialization finish!");
}
}
进行优化,移除特点等操作.
在下一个博客进行详细讲解.