VINS（八）初始化

首先通过imu预积分陀螺仪和视觉特征匹配分解Fundamental矩阵获取rotationMatrix之间的约束关系，联立方程组可以求得外参旋转矩阵；

接下来会检测当前frame_count是否达到WINDOW_SIZE，确保有足够的frame参与初始化；

bool Estimator::initialStructure();

1. 保证imu充分运动，只需要考察线加速度的变化，计算窗口中加速度的标准差，标准差大于0.25则代表imu充分激励，足够初始化（这一部分在ios版本实现中被注释掉了，不知道为什么）：

    {map<double, ImageFrame>::iterator frame_it;Vector3d sum_g;for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++){double dt = frame_it->second.pre_integration->sum_dt;Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;sum_g += tmp_g;}Vector3d aver_g;aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);// Standard deviation of linear_accelerationdouble var = 0;for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++){double dt = frame_it->second.pre_integration->sum_dt;Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);//cout << "frame g " << tmp_g.transpose() << endl;
        }var = sqrt(var / ((int)all_image_frame.size() - 1));//ROS_WARN("IMU variation %f!", var);if(var < 0.25){ROS_INFO("IMU excitation not enouth!");//return false;
        }}

2. 纯视觉初始化，对Sliding Window中的图像帧和相机姿态求解sfm问题：

　　a. 首先通过FeatureManeger获取特征匹配，考察最新的keyFrame和sliding window中某个keyFrame之间有足够feature匹配和足够大的视差（id为l），这两帧之间通过五点法恢复出R，t并且三角化出3D的feature point：

relativePose(relative_R, relative_T, l)

　　b. 3D的feature point和sliding window中的keyFrame的2D feature求解PnP，并且使用ceres优化：

sfm.construct(frame_count + 1, Q, T, l,relative_R, relative_T,sfm_f, sfm_tracked_points)

　　c. 所有的frame求解PnP

cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1)

3. imu与视觉对齐，获取绝对尺度

bool Estimator::visualInitialAlign()

　　a. 求解陀螺仪零偏metric scale，这里的metric scale指的是imu和sfm结果进行对齐需要的比例：

bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);

bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)
{solveGyroscopeBias(all_image_frame, Bgs);if(SolveScale(all_image_frame, g, x))return true;else return false;
}

　　b. 初始化成功，则对于imu数据需要repropogate，也就是从当前时刻开始预积分；同时通过三角化和上一步计算的scale可以获得每个feature的深度;

至此，视觉和imu已经对齐

转载于:https://www.cnblogs.com/shang-slam/p/7147095.html

VINS（八）初始化相关推荐

萌新改代码系列（一）--VINS+GPS
VINS与GPS组合距离上次写博客过去了快一年了,这一年来我一直忙于与SLAM方向几乎没有关系的科研工作,说来惭愧,最终也没研究出个啥.最近得空,就把我之前开源的代码整理了一下,把与GPS的组合部分 ...
一文详解单目VINS论文与代码解读目录
本文旨在对前一阶段学习vins-mono开源框架的总结.结合暑假秋招之前报名的深蓝学院的<从零开始手写VIO>课程,本文从VIO原理以及开源代码分析两部分进行详细介绍.PS:提升代码能力最 ...
VIO/VINS/VSLAM问题定位流程与思路
首先假设读者是了解基础VSLAM,了解VIO基础(至少要会标定,调过几个开源系统的) 先说一下双目的VINS-FUSION,大部分问题都是基线造成的问题,简单点说就是如果使用较短的基线如5cm(Rea ...
VSLAM 相关知识点总结
VSLAM 相关知识点这篇文章是对VSLAM涉及的知识点进行系统性的总结和更新,一些内容来源至VSLAM经典教材,博客,和开源项目(引用材料如下表) SLAM十四讲高博古月老师的技术博客崔神的g ...
视觉SLAM前端特征检测与跟踪的思考
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达就目前视觉SLAM的引用来区分,分为基于特征法的和直接法的视觉SL ...
C++应用过程中使用知识点
一读代码中遇到虚函数,此处总结虚函数的用法转载链接 https://blog.csdn.net/hackbuteer1/article/details/7558868 二 C语言枚举类型(C语言e ...
VINS_MONO+D435i Realtime运行
艰难地运行了三天,终于算是搞定了,主要的时间和精力都花在标定上了, 总结一些需要注意的点,希望能对同样使用VINS-MONO的人有帮助. 先说一下我的初衷,其实是想要使用ORBSLAM3的Mono_I ...
Canvas-lms 开源在线学习管理系统源码部署（生产版）
我们的产品: https://canvaslms.zut.edu.cn/ 开始接到部署 Canvas-lms 这个开源教学平台的任务时, 还不知道 Canvas-lms 是什么 , 网上关于他的介绍 ...
RP-VIO：面向动态环境的基于平面的鲁棒视惯融合里程计（IROS2021）
RP-VIO: Robust Plane-based Visual-Inertial Odometry for Dynamic Environments 来源:Ram K, Kharyal C, Ha ...
ROS-3DSLAM（十一）lvi-sam源代码阅读9
2021SC@SDUSC (十一)lvi-sam源代码阅读9 前置知识 vins-mono lvi-sam的视觉部分是基于vins-mono算法的.所以在阅读开始之前学习以下这个算法是很有必要的. v ...

VINS（八）初始化

VINS（八）初始化相关推荐

最新文章

热门文章