VINS-Mono 代码解析二、状态估计（estimator）第3部分
【4】视觉/惯导联合初始化
为什么要初始化？
答：
(1)视觉系统只能获得二维信息，损失了一维信息(深度),所以需要动一下，也就是三角化才能重新获得损失的深度信息；
(2)但是，这个三角化恢复的深度信息，是个“伪深度”，它的尺度是随机的，不是真实的，所以就需要IMU来标定这个尺度；
(3)要想让IMU标定这个尺度，IMU也需要动一下，得到PVQ的P；
(4)另外，IMU存在bias，视觉获得的旋转矩阵不存在bias，所以可以用视觉来标定IMU的旋转bias；
(5)需要获得世界坐标系这个先验信息，通过初始化能借助g来确定；
初始化的逻辑图如下：
上一步【3】的目的是求相机和视觉之间的外参，为什么要放在 【4】的前面呢，哈哈，因为联合初始化的时候需要用到人家，开始上程序！
/----------------------------------------------------------程序----------------------------------------------------------/
//【4】进行初始化，一般初始化只进行一次；
if (solver_flag == INITIAL)
{
//frame_count是滑动窗口中图像帧的数量，一开始初始化为0，滑动窗口总帧数WINDOW_SIZE=10
if (frame_count == WINDOW_SIZE)
{
bool result = false;
//确保有足够的frame参与初始化，有外参，且当前帧时间戳大于初始化时间戳+0.1秒
if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
{
// 4.1 视觉惯性联合初始化-包括纯视觉SfM，视觉和IMU的松耦合
result = initialStructure();
// 4.2 更新初始化时间戳-
initial_timestamp = header.stamp.toSec();
}
if(result)//初始化成功则进行一次非线性优化
{
solver_flag = NON_LINEAR;// 先进行一次滑动窗口非线性优化，得到当前帧与第一帧的位姿
solveOdometry(); // 执行非线性优化具体函数solveOdometry()
slideWindow(); // 滑动窗
// 4.5 剔除feature中估计失败的点（solve_flag == 2）0 haven't solve yet; 1 solve succ;
f_manager.removeFailures();
ROS_INFO("Initialization finish!");
// 4.6 初始化窗口中PVQ
last_R = Rs[WINDOW_SIZE];//得到当前帧与第一帧的位姿
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
else
slideWindow();// 初始化失败则直接滑动窗口
//因为只有在solveOdometry后才确定特征点的solve_flag(是否成功解算得到depth值)
}
else
frame_count++;// 图像帧数量+1，直到满足窗口数量
}/----------------------------------------------------------程序----------------------------------------------------------/
解析：
首先判断是否可以进行初始化，如果可以，看一下滑动窗口内帧的数量是否足够！如果这两个条件满足，并且有相机和IMU间的外参以及系统运行了一段时间，那么就可以初始化了，
然后初始化完成后可以进行一次非线性优化，得到位姿态；如果帧不够，就进行滑窗！
子程序 initialStructure();
概念：
主要分为：1、纯视觉SFM估计滑动窗内相机位姿和路标点逆深度。2、视觉惯性联合校准，SFM与IMU积分对齐。
【1】
（1）选择一个滑动窗，在最后一帧与滑动窗之前帧 寻找帧：跟踪到的点数目大于30个，并且视差超过20的，找到后用五点法本质矩阵初始化恢复R和t，否则滑动窗内保留最新图像帧，继续下一帧。
（2）随意设置一个尺度因子，三角化两帧观测到的所有路标点。 再用Pnp算法估计滑动窗内所有其余帧的位姿。 滑动窗内全局BA重投影误差优化所有帧位姿。
（3）假设IMU-camera外参知道，乘以视觉得到的位姿，转换到IMU坐标系下。
【2】
（1）陀螺仪零偏bg标定。
（2）速度v，重力g,尺度s初始化。
（3）重力矢量修正（优化方向）。
-------------------------------------------------------------------------------------------------
Quaterniond Q[frame_count + 1]; //旋转四元数Q
Vector3d T[frame_count + 1]; //平移矩阵T
map<int, Vector3d> sfm_tracked_points; //存储SFM重建出特征点的坐标
vector<SFMFeature> sfm_f; //SFMFeature三角化状态、特征点索引、平面观测、位置坐标、深度
all_image_frame： // 这个数据结构，它包含了滑窗内所有帧的视觉和IMU信息，它是一个hash，以时间戳为索引
为什么容量是frame_count + 1？因为滑窗的容量是10，再加上当前最新帧，所以需要储存11帧的值！
-------------------------------------------------------------------------------------------------
数据结构: vector<SFMFeature> sfm_f
它定义在initial/initial_sfm.h中，
struct SFMFeature
{
bool state;//状态（是否被三角化）
int id;
vector<pair<int,Vector2d>> observation;//所有观测到该特征点的 图像帧ID 和 特征点在这个图像帧的归一化坐标
double position[3];//在帧l下的空间坐标
double depth;//深度
};
/-------------------------------------------------------------------------------------------------------------------程序------------------------------------------------------------------------------------------------------------------------------/
（1）这一部分的思想就是通过计算滑窗内所有帧的线加速度的标准差，判断IMU是否有充分运动激励，以判断是否进行初始化
bool Estimator::initialStructure()
{
TicToc t_sfm;
{ map<double, ImageFrame>::iterator frame_it;
// 1.1 先求均值 aver_g
Vector3d sum_g;//sum_g需要置0,bug fixed。
//(第一次循环，求出滑窗内的平均线加速度
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
{
//all_image_frame这个数据结构，见5.2-2，它包含了滑窗内所有帧的视觉和IMU信息
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;
//总帧数减1，因为all_image_frame第一帧不算
aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);
//1.2 再求标准差var,表示线加速度的标准差。
double 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));//计算标准差
//以标准差判断可观性
if(var < 0.25)
{
ROS_INFO("IMU excitation not enouth!");
//return false;
//这里不能简单的返回false,这样会导致sfm中pnp无解,如果要返回false就需要改滑窗的特征点管理
}
}疑问：为什么要用这种方法判断一下视觉 IMU 是否可以初始化？不能直接初始化吗？
答：联合初始化的核心思想就是利用运动去求参数，比如相机和IMU都是安装在载体上，那么他们实际的距离一定是一样的，只是测量出的距离不一样而以;
故可以利用距离相等求出IMU 的bias, 而旋转也是一个道理！
（2）滑窗内全局的SFM
Quaterniond Q[frame_count + 1];//Twc由相机系到世界系的变换,也就是在世界系(参考关键帧)下的位姿
Vector3d T[frame_count + 1];
map<int, Vector3d> sfm_tracked_points;//feature_id + 三角化且优化后的点坐标
vector<SFMFeature> sfm_f;
//2.1: SFMFeature结构体，它存放着一个特征点的所有信息。容器定义完了，接下来就是往容器里放数据。
for (auto &it_per_id : f_manager.feature)//对于滑窗中出现的所有特征点
{
int imu_j = it_per_id.start_frame - 1;//从start_frame开始帧编号
SFMFeature tmp_feature;
tmp_feature.state = false; //状态（是否被三角化）
tmp_feature.id = it_per_id.feature_id;//特征点id
//对于当前特征点 在每一帧的坐标
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;//帧编号+1
Vector3d pts_j = it_per_frame.point;//当前特征在编号为imu_j++的帧的归一化坐标
//该feature在哪几帧中被观测到,同时记录其在滑动窗口中的位置
tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}));//把当前特征在当前帧的坐标和当前帧的编号配上对
//tmp_feature.observation里面存放着的一直是同一个特征，每一个pair是这个特征在 不同帧号 中的 归一化坐标。
}
sfm_f.push_back(tmp_feature);//sfm_f里面存放着是不同特征
}
//在滑窗(0-9)中找到第一个满足要求的帧(第l帧)，它与最新一帧(frame_count=10)有足够的共视点和平行度，并求出这两帧之间的相对位置变化关系；
Matrix3d relative_R;
Vector3d relative_T;//当前帧到共视帧
int l;//滑窗中满足与最新帧视差关系的那一帧的帧号l
//2.2 a.计算滑窗内的每一帧(0-9)与最新一帧(10)之间的视差，直到找出第一个满足要求的帧，作为我们的第l帧；
if (!relativePose(relative_R, relative_T, l))
{
//这里的第L帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧l，
//会作为 参考帧 到下面的全局sfm 使用，得到的Rt为当前帧到第l帧的坐标系变换Rt。
ROS_INFO("Not enough features or parallax; Move device around");
return false;
}
//construct()：对窗口中每个图像帧求解sfm问题，
// 得到所有图像帧相对于参考帧l的姿态四元数Q、平移向量T和特征点坐标sfm_tracked_points （核心）
// 看一下*construct()*里面干了什么。见initial/initial_sfm.cpp文件
GlobalSFM sfm;
if(!sfm.construct(frame_count + 1, Q, T, l,
relative_R, relative_T,
sfm_f, sfm_tracked_points))
{
//这说明了在初始化后期的第一次slidingwindow() marg掉的是old帧。
ROS_DEBUG("global SFM failed!");
marginalization_flag = MARGIN_OLD;
return false;
}
sfm.construct() 这个函数就是对窗口中每个图像帧求解sfm问题，得到所有图像帧相对于参考帧C0的旋转四元数Q、平移向量T和特征点坐标(核心！)，
这一部分就不细说了，可以参考 高翔 14讲 或者下面的连接：
https://blog.csdn.net/xholes/article/details/79935700 然后我们进行下一部分：
理论部分：
目的：利用SfM确定各个pose 和 特征点的相对于c0帧的位置关系
这一部分和基于图像的三维重建比较像，可以用三角化和PnP把这一串的ck帧的位姿和特征点位置确定下来(特征点是伪深度)，在加上外参数qbc和pbc，一系列bk帧的位姿也确定下来。注意，这里把c0帧作为基础帧，实际上，c0帧旋转一下使gc0和gw方向一致时获得的坐标系就是vins的世界坐标系，也就是先验，这一部分也是很重要的部分。
先推导公式，所有帧的位姿(Rc0ck,qc0ck)表示相对于 第一帧相机坐标系(·)c0。相机到IMU的外参为(Rbc,qbc)，得到姿态从相机坐标系转换到IMU坐标系的关系。
将T展开有成R与p有：
左侧矩阵的两项写开：
/-------------------------------------------------------------------------------------------------------------------程序------------------------------------------------------------------------------------------------------------------------------/
（3）视觉/IMU 的联合初始化
1、利用相机旋转约束标定IMU角速度bias，求解的目标函数如下公式所示：
在SfM完成且外参数标定完之后，头两个值是已知的了，而且我们假设头两个值是准的。理想状态下，这三个数乘积应该是单位四元数，很可惜，第三个值是IMU预积分得到的，而预积分里面是有bias的。
所以，通过最小化这个目标函数的，可以把旋转bias标定出来！在IMU预积分部分，有：
带入到损失函数里，可以得到：
或者是：
带入bias的残差后，得到，
实部没有需要标定的量，所以只用考虑虚部，也就是：
两侧再乘以 ，可以构造出Ax=B的形式，在采用LDLT分解，就可以求出状态量：
其实这里不像那种用高斯牛顿法迭求解，而更像是用直接法,也就是矩阵运算的方式来求待优化的状态量，6.1.4也是一样的思路。
代具体代码见:initial_aligment.cpp 函数 solveGyroscopeBias()
/-----------------------------------------------------------------------------------------------程序----------------------------------------------------------------------------------------------/
void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
Matrix3d A;
Vector3d b;
Vector3d delta_bg;
A.setZero();
b.setZero();
map<double, ImageFrame>::iterator frame_i;//第i帧
map<double, ImageFrame>::iterator frame_j;//第j帧
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
{
frame_j = next(frame_i);//j帧等于i帧的下一帧。
MatrixXd tmp_A(3, 3);
tmp_A.setZero();
VectorXd tmp_b(3);
tmp_b.setZero();
//视觉给出的i帧->j帧的旋转(四元数),对应公式(2)的右侧,公式(2)指的是上面给出注释的公式编号.
//qc0_bk' * qc0_bk+1.
Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
// IMU预积分中旋转对gyro bias的jacobian
tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
//对应公式(3)的右侧
tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
//对应公式(5)
A += tmp_A.transpose() * tmp_A;
b += tmp_A.transpose() * tmp_b;
}
//Cholesky分解求解线性方程，svd分解求解线性方程；Cholesky分解要求矩阵为对称且正定的。
delta_bg = A.ldlt().solve(b);
ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());
//因为求解出的Bias是变化量，所以要累加，这个地方滑窗内的累加深入理解。
//这里并不是滑窗内累加,而是滑窗内每一帧的bias更新。
for (int i = 0; i <= WINDOW_SIZE; i++)
Bgs[i] += delta_bg;
//利用新的Bias重新repropagate。
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
{
frame_j = next(frame_i);
frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
//此处初始的加表零偏设置为0了，因为被重力吸收了，开始很小！
//这里为什么是第一帧的bias,滑窗内的bias都相同?
}
}/-----------------------------------------------------------------------------------------------程序----------------------------------------------------------------------------------------------/
把程序和理论结合起来，认真的研读！
理论部分
上一篇： table中合并单元格的最大限制
下一篇： 线下教培机构的用户增长模型 学习笔记