VINS-Mono源码解析（四）后端: Initialization

整个Initialization部分对应Estimator::initialStructure()函数, 包括Vision-only SFM和Visual Inertial Alignment两部分, 它们的实现分别在GlobalSFM类和VisualIMUAlignment()函数中.
initialStructure()函数的内部流程如下:
1. 先判断IMU是否有足够的variance, 求all_image_frame中所有帧的平均加速度, 并计算加速度的方差, 判断方差是否足够大
2. 调用relativePose判断sliding window是否某一帧l与当前帧视差足够大, 如果存在的话, 则根据特征匹配用五点法计算l和当前帧的relative R,t, 用来做后面的SFM
3. 调用construct()进行SFM, 估计slidingwindow中的camera pose和features的3D position.
4. 然后对all_image_frame中没有参与SFM的帧的位姿进行了PnP求解.
这里暂时还没太弄明白all_image_frame的含义, 除了SFM的帧, 其他还有哪些帧?
5. 调用visualInitialAlign()进行视觉和IMU的校准.

1. Vision-only SFM: GlobalSFM::construct()

　　Vision-only SFM具体实现在GlobalSFM类中. initialStructure()先调用relativePose()来判断slidingwindow中是否存在某一帧l和当前帧(即最后一帧)的视差足够大, 如果有足够大的视差, 则计算l帧和当前帧的relative R,t, 然后进行后面的SFM初始化,也就是调GlobalSFM::construct()函数.
　　GlobalSFM在initial_sfm.h中,里面还有另外两个类:
- SFMFeature是SFM中的特征点,包括id,state(是否被三角化),各帧中观测,3D坐标等.
- ReprojectionError3D 定义了重投影误差计算函数, 供ceres优化用的, ceres要求重载一个()操作符的模板函数.

GlobalSFM声明如下:

class GlobalSFM{public:    GlobalSFM();    /*\brief 外部调用接口,主要处理函数. 输入第l帧和最后一帧的相对R,t, 根据特征点的观测估计所有帧的位姿和特征点的3D坐标    \param[in] frame_num: pose的个数, elements in q,T    \param[out] q: SFM结果,每帧在l帧参考系下的quaternion    \param[out] T: SFM结果,每帧在l帧参考系下的position    \param[in] l: 以第l帧为参考系,即l帧的pose为坐标原点    \param[in] relative_R: 第l帧到最后一帧的相对旋转    \param[in] relative_T: 第l帧到最后一帧的相对平移    \param[in] sfm_f: feature list,每个SFMFeature中包含多个观测    \param[out] sfm_tracked_point: 优化后的3D特征点在l帧参考系的position    */    bool construct(int frame_num, Quaterniond* q, Vector3d* T, int l,              const Matrix3d relative_R, const Vector3d relative_T,              vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points);private:    /**\brief 根据当前已经三角化的特征点估计某一帧的R,t */    bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i, vector<SFMFeature> &sfm_f);    /**\brief 输入两个pose和2D观测点, 三角化3D特征点 */    void triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,                            Vector2d &point0, Vector2d &point1, Vector3d &point_3d);    /**\brief 输入两帧的pose, 三角化它们共同观测的特征点, 之前已经被三角化的特征点不再处理 */    void triangulateTwoFrames(int frame0, Eigen::Matrix<double, 3, 4> &Pose0,                               int frame1, Eigen::Matrix<double, 3, 4> &Pose1,                              vector<SFMFeature> &sfm_f);    int feature_num;};

其中,construct()函数的内部流程如下:
- step 1: 所有pose和feature点的初始化, 以第l帧为参考坐标系
1. 先第l帧和最后一帧做三角化,根据三角化的点PnP求解第l+1的位姿,再l+1和最后一帧三角化,再PnP求l+2位姿,依次求到倒数第二帧的位姿.
2. 第l+1帧~倒数第二帧的所有帧与第l帧三角化剩下的特征点
3. 依次PnP求解第l-1帧~第0帧的位姿并将第l-1帧~第0帧 与 第l帧做三角化.
4. 遍历所有特征点, 对那些还没有三角化的点,如果观测大于2个, 根据首尾观测进行三角化.
- step 2: Local Bundle Adjustment, ceres
5. ceres求解, residuals用的特征点到每一帧重投影误差, 将求解结果写出到q,T和sfm_tracked_points

2. Visual Inertial Alignment: Estimator::visualInitialAlign()

1) Gyroscope Bias Calib: solveGyroscopeBias()

　　gyro的标定思路是:对于相邻的两帧, vision有一个相对旋转q, IMU预积分有一个相对旋转γ, Align的目的就是寻找一个最好的δbw使得两个相对旋转尽可能接近, q是不变的, δbw的变化使得γ变化, 变化”斜率”就是γ对δbw的JacobianJγbw, 对应论文中的公式为:

minδbw∑k∈B∣∣∣∣∣∣∣∣ qbk+1bk⨂γ^bkbk+1⨂⎡⎣112Jγbwδbw⎤⎦ ∣∣∣∣∣∣∣∣2

其中, qbk+1bk是视觉给出的k到k+1的旋转四元数, γ^bkbk+1是IMU预积分得到的k+1到k的旋转四元数, δbw就是要标定的gyro bias的变化量, 上式可formulate成线性等式约束Ax=b的形式,

Jγbwδbw=2(qbk+1bk⨂γ^bkbk+1)−1

左边的jacobian就是代码中的tmp_A, 右边对应代码中的tmp_b, 每两帧之间都可以构造一个这样的等式约束, 超定方程用ATAx=ATb求得最小二乘解, 注意求得的δbw是变化量, 所以将每帧的Bgs[i]都加上该delta量进行更新. 然后利用更新后的gyro bias重新repropagate.

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();    // 视觉给出的i帧->j帧的旋转(四元数)    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);    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;}delta_bg = A.ldlt().solve(b);

2) Velocity, Gravity Vector, Metric Scale Initialization: LinearAlignment()

这部分标定需要标定的未知量有(向量长度=3*N+3+1)

XI=[vb0b0,vb1b1,...,vbnbn,gc0,s]

貌似论文有好几个版本, 有一个版本是估计bk在c0下的速度, 其实差不多, 二者就差一个Rc0bk的旋转.
标定思路为: IMU预积分的结果和SFM的结果之间的关系可写成下式:

α^bkbk+1β^bkbk+1=Rbkc0(s(p¯c0bk+1−p¯c0bk)+12gc0Δt2k−Rc0bkvbkbkΔtk)=Rbkc0(Rc0bk+1vbk+1bk+1+gc0Δtk−Rc0bkvbkbk)

等式左边是IMU预积分, 右边是SFM的结果和要标定的量. 其中, α^bkbk+1,β^bkbk+1分别是IMU预积分得到的第k+1帧body系的position和velocity, 在第k帧body系的参考坐标系下.右上标表示参考坐标系,右下标表示物理量对象,同理vbkbk是第k帧的body系velocity. 上面两个公式实际上是先在c0系下计算出pos和vel, 然后再乘以Rbkc0转到bk系.

下面将上式改写成Ax=b的等式约束形式, 式中, 未知数为vbkbk,vbk+1bk+1,gc0,s, 向量长度为10(3+3+3+1). α^bkbk+1,β^bkbk+1从IMU预积分拿到, Rbkc0,Rc0bk+1,p¯c0bk+1,p¯c0bk从vision SFM拿到, 不过要做一下转换, 因为SFM拿到的是相机的R,t, 要通过相机外参pbc,qbc转成body(即IMU)的R,t. 转换公式为

qc0bksp¯c0bk=qc0ck⨂(qbc)−1=sp¯c0ck−Rc0bkpbc

将外参转换公式带到前面的式子中, 可以写成如下形式, 直接上图把,懒得敲公式了
每相邻两帧都有一个上面的等式约束对应代码中的tmp_A6×10x10×1=tmp_b6×1. 因为是超定方程,所以要转化一下A′=ATA, b′=ATb, 然后填到对应的位置.最后统一求解.

注意:公式中有10个未知数, 只有6个等式, 为什么是超定呢? 是因为对每一帧来说,除去首尾帧, 都有两组这个等式约束. 总的未知数是3n+3+1个, 约束方程的个数是6(n−1)个, 当n>2, 3n+3+1<6(n−1), 所以超定.

Alignment求解代码:

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);  // 10 = 3(vel_i)+3(vel_j)+3(gravity)+1(scale)    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];    //cout << "delta_p   " << frame_j->second.pre_integration->delta_p.transpose() << endl;    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;    //cout << "delta_v   " << frame_j->second.pre_integration->delta_v.transpose() << endl;    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;  //r_A 10*10    VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;  //r_b 10*1    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>();}

这里有一处不解: 代码中有一处地方除100, 后面方程求解前A,b都乘以了1000,不知是为何? 是有numerical issue吗?

3)Gravity Refinement: RefineGravity()

　　上一步求出重力向量可以进一步refine, 因为重力加速度的模值通常是已知的(约9.8), 所以可以用重力加速度大小这个prior去refine重力向量, 但||gc0||=9.8是一个非线性的等式约束, 很难直接加到上面的线性约束中求解. 论文中将其转化为线性约束, 思路是: 设g的模值G已知, 令g=Geg+w1e1+w2e2, 其中e_g是上一步求出的g^的方向向量(单位向量), e1,e2是张成eg的正切空间的两个正交单位向量. 然后将Geg+w1e1+w2e2带到2)中的那个约束方程中, 这是g的未知数从之前的3个就变成了w1,w2两个, 然后重新求解2)中的线性方程, 又得到一个新的g^, 又重复这一步骤多次后直到g^收敛, 代码中重复了4次.

*一开始有点不理解，若w1,w2不为０, g的模值不就大于Ｇ了吗？
看代码后发现, 每次估计完g以后, 都会做一次归一化使得其模值为G, 所以可以理解为这里的refine其实是refine重力的方向, 同时refine其他估计量. 因为方向基本不会差太多, w1,w2通常很小, 所以可以做这样的近似.*

至此,IMU和VisionSFM算是标定完成, 这里漏了一块IMU和相机的互标定, 也就是相机外参估计, 主要就标一下Ric,据说tic对系统影响不大,而且也不太好标,需要有比较大的平移量才可以标定出来