6.初始化 第一个问题，为什么要初始化？ 对于单目系统而言， (1)视觉系统只能获得二维信息，损失了一维信息(深度),所以需要动一下，也就是三角化才能重新获得损失的深度信息； (2)但是，这个三角化恢复的深度信息，是个“伪深度”，它的尺度是随机的，不是真实的，所以就需要IMU来标定这个尺度； (3)要想让IMU标定这个尺度，IMU也需要动一下，得到PVQ的P； (4)另外，IMU存在bias，视觉获得的旋转矩阵不存在bias，所以可以用视觉来标定IMU的旋转bias； (5)需要获得世界坐标系这个先验信息，通过初始化能借助g来确定； 对于双目系统而言， 初始化的难度会低一些，因为双目可以一下确定深度，只需要通过g的方向(先验)来确定世界坐标系。 注意，这个初始化只进行一次就够了，或者是在系统重启时执行一次，大部分时候，系统都处于NON_LINEAR的状态。因为，初始化的时候，就能确定尺度scaler和bias初始值，scaler确定后，在初始化获得的这些路标点都是准的了，后续通过PnP或者BA得到的特征点都是真实尺度的了。而bias初始值确定以后，在后续的非线性优化过程中，会实时更新。
6.1 基础原理
初始化的逻辑图如下：
6.1.1 如果旋转外参数 qbc 未知, 则先估计旋转外参数 实际上讲，这部分并不是vins代码中初始化的内容，而是初始化之前就需要完成的判断和操作，见5.2-4。根据IMU和视觉部分的旋转关系，可以得到下面的关系式：
将多个帧之间的等式关系一起构建超定方程Ax=0。对A进行svd分解，其中最小奇异值对应的奇异向量便为需要求解的qbc。 虽然这里IMU的旋转部分并没有标定，得到的外参数可能不太准。但是问题不大，因为初始化所占用的总运行时间不长，而更长生命周期的后端会持续的优化这部分的值。 6.1.2 利用SfM确定各个pose和特征点的相对于c0帧的位置关系 这一部分和基于图像的三维重建比较像，可以用三角化和PnP把这一串的ck帧的位姿和特征点位置确定下来(特征点是伪深度)，在加上外参数qbc和pbc，一系列bk帧的位姿也确定下来。注意，这里把c0帧作为基础帧，实际上，c0帧旋转一下使gc0和gw方向一致时获得的坐标系就是vins的世界坐标系，也就是先验。在vins代码中，这一部分篇幅很长，但是这一部分也是很重要的部分。
首先我们先推导论文式(14)，所有帧的位姿(Rc0ck,qc0ck)表示相对于第一帧相机坐标系(·)c0。相机到IMU的外参为(Rbc,qbc)，得到姿态从相机坐标系转换到IMU坐标系的关系。
将T展开有成R与p有：
左侧矩阵的两项写开：
6.1.3 利用相机旋转约束标定IMU角速度bias 求解的目标函数如下公式所示：
在SfM完成且外参数标定完之后，头两个值是已知的了，而且我们假设头两个值是准的。理想状态下，这三个数乘积应该是单位四元数，很可惜，第三个值是IMU预积分得到的，而预积分里面是有bias的。所以，通过最小化这个目标函数的，可以把旋转bias标定出来！ 在IMU预积分部分，也就是4.1.1最后的那个公式，有：
带入到损失函数里，可以得到：
或者是：
带入bias的残差后，得到，
实部没有需要标定的量，所以只用考虑虚部，也就是：
两侧再乘以 ，可以构造出Ax=B的形式，在采用LDLT分解，就可以求出状态量：
其实这里不像那种用高斯牛顿法迭求解，而更像是用直接法,也就是矩阵运算的方式来求待优化的状态量，6.1.4也是一样的思路。代具体代码见:initial_aligment.cpp 函数 solveGyroscopeBias()。 接下来是代码解析， (1)参数的传入和容器的定义
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;
map<double, ImageFrame>::iterator frame_j;
上式中，A和b对应的是Ax=b。注意，传入的参数是all_image_frame，不仅仅是滑窗内的帧。frame_i和frame_j分别读取all_image_frame中的相邻两帧。 (2)套本小节的最后一个公式，构造Ax=b等式
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
{
frame_j = next(frame_i);
MatrixXd tmp_A(3, 3);
tmp_A.setZero();
VectorXd tmp_b(3);
tmp_b.setZero();
Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
A += tmp_A.transpose() * tmp_A;
b += tmp_A.transpose() * tmp_b;
}
注意到本小节的第一个公式了吗，那里有求和，所以需要遍历all_image_frame，然后叠加A和b。 (3)ldlt分解
delta_bg = A.ldlt().solve(b);
(4)给滑窗内的IMU预积分加入角速度bias
for (int i = 0; i <= WINDOW_SIZE; i++)
Bgs[i] += delta_bg;
(5)重新计算所有帧的IMU积分(重要！)
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]);
}
}
repropagate()部分内容见4.3.2.。 6.1.4 利用IMU的平移估计重力/各bk帧速度/尺度scaler 首先要明确需要优化的状态量是什么，是各帧在bk坐标系下的速度，c0帧下的g和SfM的尺度scaler：
这块有个遗留问题，就是为什么要优化速度呢？ 在IMU预积分部分，已经有如下的公式：
但是w坐标系我们不知道，只知道c0坐标系，所以需要把上面的公式转到c0坐标系上：
上式中，等号左边减去等号右边就是残差，理想状态下，这个残差是0，那么带入上式得：
把这个 XX = 0的等式也转为 A x = b这种线性方程组的形式，如下式：
或者矩阵的形式：
对于beta而言，也是类似的，
那么，把这两个矩阵合体，就能得到论文中的等式：
同样采用LDLT分解，就能求出状态量：
接下来是代码解析， (1)参数的传入和容器的定义
bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
int all_frame_count = all_image_frame.size();
int n_state = all_frame_count * 3 + 3 + 1; //需要优化的状态量的个数
MatrixXd A{n_state, n_state};
A.setZero();
VectorXd b{n_state};
b.setZero();
map<double, ImageFrame>::iterator frame_i;
map<double, ImageFrame>::iterator frame_j;
上式中，A和b对应的是Ax=b。注意，传入的参数是all_image_frame，不仅仅是滑窗内的帧。 frame_i和frame_j分别读取all_image_frame中的相邻两帧。 (2)套本小节的最后一个公式，构造Ax=b等式
int i = 0;
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)
{
frame_j = next(frame_i);
MatrixXd tmp_A(6, 10);
tmp_A.setZero();
VectorXd tmp_b(6);
tmp_b.setZero();
double dt = frame_j->second.pre_integration->sum_dt;
tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();
tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;
tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0];
tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();
tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v;
Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
cov_inv.setIdentity();
MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;
A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
b.segment<6>(i * 3) += r_b.head<6>();
A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();
b.tail<4>() += r_b.tail<4>();
A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();
A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();
}
注意，作者在这里还非常机智地加上了信息矩阵cov_inv。 TODO 但是这些乘1000，除100的操作，最后算起来为啥不等于1呢？ 另外需要注意的地方是，**虽然公式中只计算了相邻2帧，但是代码中，它把所有帧都放进去了，因此，A矩阵和b向量是远大于公式中的尺寸，而且在代码中有一个叠加的操作！非线性优化里构造H矩阵和J矩阵也是一样的。**这里是我看代码前比较疑惑的地方。 (3)ldlt分解，得到尺度和g的初始值，并用先验判断
x = A.ldlt().solve(b);
double s = x(n_state - 1) / 100.0;
g = x.segment<3>(n_state - 4);
if(fabs(g.norm() - G.norm()) > 1.0 || s < 0)
{//如果重力加速度与参考值差太大或者尺度为负则说明计算错误
return false;
}
(4) 利用gw的模长已知这个先验条件进一步优化gc0
RefineGravity(all_image_frame, g, x);
s = (x.tail<1>())(0) / 100.0;
(x.tail<1>())(0) = s;
ROS_DEBUG_STREAM(" refine " << g.norm() << " " << g.transpose());
if(s < 0.0 )
return false;
else
return true;
}
见下一节。虽说是优化gc0，实际上都优化了。 6.1.5 利用gw的模长已知这个先验条件进一步优化gc0 但是，这里有一个bug，就是把g当成3自由度的向量优化，但是实际上g只有2自由度，因为它的模长是已知的。所以，在这里，需要想一个办法，就是怎么样用一个2自由度的表达式来表示3维的向量呢？
在这里，采用球面坐标进行参数化，也就是用g的模长作为半径画一个半球，上图蓝色线对应的是gc0的测量值的方向(也就是优化前的方向)，在这个交点上找到一个切平面，用gc0，b1，b2构造一个坐标系，那么在轴b1和b2上坐标值w1和w2就是我们需要求的量。求出来之后，把等号右边加在一起，就是优化后的gc0值。 注意，b1的方向是由gc0的测量值的方向与[1,0,0]作叉乘得到的，b2的方向是由gc0的测量值的方向与b1作叉乘得到的。 这样，6.1.4公式得到了更新，此时带优化量减少了1维：
即 ，同样使用LDLT分解：
其实，在这里，我们可以发现这一部分做的工作和6.1.4是相似的，但是却是在6.1.4的基础上进一步做的工作。 接下来是代码解析， (1)参数的传入和容器的定义
void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
Vector3d g0 = g.normalized() * G.norm();
Vector3d lx, ly;
//VectorXd x;
int all_frame_count = all_image_frame.size();
int n_state = all_frame_count * 3 + 2 + 1;
MatrixXd A{n_state, n_state};
A.setZero();
VectorXd b{n_state};
b.setZero();
map<double, ImageFrame>::iterator frame_i;
map<double, ImageFrame>::iterator frame_j;
上式中，A和b对应的是Ax=b。注意，传入的参数是all_image_frame，不仅仅是滑窗内的帧。 frame_i和frame_j分别读取all_image_frame中的相邻两帧。 (2)一共迭代四次求解，并构建且向空间
for(int k = 0; k < 4; k++)
{
MatrixXd lxly(3, 2);
lxly = TangentBasis(g0);
....(3)
}
切向空间的构建如下代码所示：
MatrixXd TangentBasis(Vector3d &g0)
{
Vector3d b, c;
Vector3d a = g0.normalized();
Vector3d tmp(0, 0, 1);
if(a == tmp)
tmp << 1, 0, 0;
b = (tmp - a * (a.transpose() * tmp)).normalized();
c = a.cross(b);
MatrixXd bc(3, 2);
bc.block<3, 1>(0, 0) = b;
bc.block<3, 1>(0, 1) = c;
return bc;
}
(3)套本小节的最后一个公式，构造Ax=b等式
int i = 0;
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)
{
frame_j = next(frame_i);
MatrixXd tmp_A(6, 9);
tmp_A.setZero();
VectorXd tmp_b(6);
tmp_b.setZero();
double dt = frame_j->second.pre_integration->sum_dt;
tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
tmp_A.block<3, 2>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly;
tmp_A.block<3, 1>(0, 8) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;
tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0] - frame_i->second.R.transpose() * dt * dt / 2 * g0;
tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
tmp_A.block<3, 2>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly;
tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0;
Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
//cov.block<6, 6>(0, 0) = IMU_cov[i + 1];
//MatrixXd cov_inv = cov.inverse();
cov_inv.setIdentity();
MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;
A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
b.segment<6>(i * 3) += r_b.head<6>();
A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>();
b.tail<3>() += r_b.tail<3>();
A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>();
A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>();
}
(4)ldlt分解，得到优化后的状态量x
A = A * 1000.0;
b = b * 1000.0;
x = A.ldlt().solve(b);
VectorXd dg = x.segment<2>(n_state - 3);
g0 = (g0 + lxly * dg).normalized() * G.norm();
//double s = x(n_state - 1);
}
g = g0;
}
6.1.6 利用gc0和gw确定世界坐标系 (1) 找到 c0 到 w 系的旋转矩阵 Rwc0 = exp([θu])
gc0已经求出来了，而gw一直就是一个已知的量，因此它们之间的夹角θ是能很快求出来的； 所以我们然后可以用gc0和gw作叉乘得到一个旋转轴u； 最后把c0坐标系，绕着转轴旋转一个θ，就能找到c0 到 w 系的对齐关系，也就是Rwc0 = exp([θu])。 (2) 把所有 c0 坐标系下的变量旋转到 w系下 所有量都乘上Rwc0就可以了。我们定义的c0 与 w 系的原点坐标是重合的。 (3) 把相机平移和特征点尺度恢复到米制单位 初始化大功告成！
