VINS-Mono代码精简版代码详解-无ROS依赖(二)

视觉惯性联合初始化

其对应的函数为

visualInitialAlign()

是在初始窗口中的图像帧完成SFM三维重建之后，即各图像帧在参考坐标系下的初始位姿都已经计算完成之后，执行的。该函数主要实现了陀螺仪的偏置校准(加速度偏置没有处理)，计算速度V[0:n]、重力g、尺度s。
同时更新了Bgs后，IMU测量量需要repropagate；得到尺度s和重力g的方向后，需更新所有图像帧在世界座标系下的Ps、Rs、Vs。
1、 计算陀螺仪偏置，尺度，重力加速度和速度

bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);
    if(!result)
    {
        ROS_DEBUG("solve g failed!");
        return false;
    }

2、 获取所有图像帧的位姿Ps、Rs，并将其置为关键帧

for (int i = 0; i <= frame_count; i++)
    {
        Matrix3d Ri = all_image_frame[Headers[i].stamp.toSec()].R;
        Vector3d Pi = all_image_frame[Headers[i].stamp.toSec()].T;
        Ps[i] = Pi;
        Rs[i] = Ri;
        all_image_frame[Headers[i].stamp.toSec()].is_key_frame = true;
    }

3、 重新计算所有特征点的深度

  //将所有特征点的深度置为-1
    VectorXd dep = f_manager.getDepthVector();
    for (int i = 0; i < dep.size(); i++)
        dep[i] = -1;
    f_manager.clearDepth(dep);
    //重新计算特征点的深度
    Vector3d TIC_TMP[NUM_OF_CAM];
    for(int i = 0; i < NUM_OF_CAM; i++)
        TIC_TMP[i].setZero();
    ric[0] = RIC[0];
    f_manager.setRic(ric);
    f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));

4、 陀螺仪的偏置bgs改变，重新计算预积分

for (int i = 0; i <= WINDOW_SIZE; i++)
    {
        pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
    }

5、 将Ps、Vs、depth尺度s缩放后转变为相对于第0帧图像座标系下
论文提到的以第一帧c0为基准座标系，通过相机座标系ck位姿得到IMU座标系bk位姿的公式为：
$$\begin{gathered} {\mathbf{q}}_{b_k}^{c_0}={\mathbf{q}}_{c_k}^{c_0}\otimes ({\mathbf{q}}_c^b{)}^{-1} \\ s{\bar{\mathbf{p}}}_{b_k}^{c_0}=s{\bar{\mathbf{p}}}_{c_k}^{c_0}-{\mathbf{R}}_{b_k}^{c_0}{\mathbf{p}}_c^b \end{gathered}$$
之前都是以第l帧为基准坐标系，转换到以第一帧b0为基准坐标系的话应该是：
$$s{\mathbf{p}}_{b_k}^{b_0}=s{\mathbf{p}}_{b_k}^{c_l}-s{\mathbf{p}}_{b_0}^{c_l}=(s{\mathbf{p}}_{c_k}^{c_l}-{\mathbf{R}}_{b_k}^{c_l}{\mathbf{p}}_c^b)-(s{\mathbf{p}}_{c_0}^{c_l}-{\mathbf{R}}_{b_0}^{c_l}{\mathbf{p}}_c^b)$$

for (int i = frame_count; i >= 0; i--)
       Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);    
   int kv = -1;
   map<double, ImageFrame>::iterator frame_i;
   for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)
   {
       if(frame_i->second.is_key_frame)
       {
           kv++;
           Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);
       }
   }
   for (auto &it_per_id : f_manager.feature)
   {
       it_per_id.used_num = it_per_id.feature_per_frame.size();
       if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
           continue;
       it_per_id.estimated_depth *= s;
   }

6、 通过将重力旋转到z轴上，得到世界座标系与摄像机座标系c0之间的旋转矩阵rot_diff

Matrix3d R0 = Utility::g2R(g);
   double yaw = Utility::R2ypr(R0 * Rs[0]).x();
   R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
   g = R0 * g;
   Matrix3d rot_diff = R0;

7、 所有变量从参考座标系c0旋转到世界座标系w

  for (int i = 0; i <= frame_count; i++)
    {
        Ps[i] = rot_diff * Ps[i];
        Rs[i] = rot_diff * Rs[i];
        Vs[i] = rot_diff * Vs[i];
    }

陀螺仪偏置，尺度，重力加速度和速度初始化

所有帧的位姿$({\mathbf{R}}_{c_k}^{c_0},{\mathbf{q}}_{c_k}^{c_0})$表示相对于第一帧相机位姿态的坐标系。相机到IMU的外参为$({\mathbf{R}}_c^b,{\mathbf{q}}_c^b)$，则有
$$\begin{gathered} {\mathbf{T}}_{c_k}^{c_0}={\mathbf{T}}_{b_k}^{c_0}{\mathbf{T}}_c^b \\ {\mathbf{R}}_{c_k}^{c_0}={\mathbf{R}}_{b_k}^{c_0}{\mathbf{R}}_c^b={\mathbf{R}}_{b_k}^{c_0}({\mathbf{R}}_c^b{)}^{-1} \\ s{\mathbf{p}}_{c_k}^{c_0}={\mathbf{R}}_{b_k}^{c_0}{\mathbf{p}}_c^b+s{\mathbf{p}}_{b_k}^{c_0}⟹s{\mathbf{p}}_{b_k}^{c_0}=s{\mathbf{p}}_{c_k}^{c_0}-{\mathbf{R}}_{b_k}^{c_0}{\mathbf{p}}_c^b \end{gathered}$$
VisualIMUAlignment()中调用了两个函数，分别用于对陀螺仪的偏置进行标定，以及估计尺度/重力和速度。

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

陀螺仪偏置标定 solveGyroscopeBias()

对于窗口中的连续两帧$b_k$和$b_{k+1}$，已经从视觉SFM中得到了旋转${\mathbf{q}}_{b_k}^{c_0}$和${\mathbf{q}}_{b_{k+1}}^{c_0}$，从预积分中得到了相邻两帧旋转${\hat{\gamma }}_{b_{k+1}}^{b_k}$，根据约束方程，建立所有相邻帧最小代价函数
$$\min \sum _{k\in \mathcal{B}}\Vert {{\mathbf{q}}_{b_{k+1}}^{c_0}}^{-1}\otimes {\mathbf{q}}_{b_k}^{c_0}\otimes {\gamma }_{b_{k+1}}^{b_k}\Vert$$
其中，对陀螺仪偏置求IMU预积分项线性化，有
$${\gamma }_{b_{k+1}}^{b_k}\approx {\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_w}^{\gamma }\delta b_w\end{array}\right]$$
在具体实现的时候，上述约束方程为
$${{\mathbf{q}}_{b_{k+1}}^{c_0}}^{-1}\otimes {\mathbf{q}}_{b_k}^{c_0}\otimes {\gamma }_{b_{k+1}}^{b_k}=\left[\begin{array}{c}1\\ 0\end{array}\right]$$
有：
$${\gamma }_{b_{k+1}}^{b_k}={{\mathbf{q}}_{b_k}^{c_0}}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^{c_0}\otimes \left[\begin{array}{c}1\\ 0\end{array}\right]$$
代入上一阶展开式，有
$${\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_w}^{\gamma }\delta b_w\end{array}\right]={{\mathbf{q}}_{b_k}^{c_0}}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^{c_0}\otimes \left[\begin{array}{c}1\\ 0\end{array}\right]$$
只考虑虚部，有：
$${\mathbf{J}}_{b_w}^{\gamma }\delta b_w=2({{\hat{\gamma }}_{b_{k+1}}^{b_k}}^{-1}\otimes {{\mathbf{q}}_{b_k}^{c_0}}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^{c_0})$$
两侧乘以${{\mathbf{J}}_{b_w}^{\gamma }}^T$，用LDLT分解求得$\delta b_w$.
在代码中，Quaterniond q_ij即${\mathbf{q}}_{b_{k+1}}^{b_k}={{\mathbf{q}}_{b_k}^{c_0}}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^{c_0}$，tmp_A即${\mathbf{J}}_{b_w}^{\gamma }$，tmp_B即$2({{\hat{\gamma }}_{b_{k+1}}^{b_k}}^{-1}\otimes {{\mathbf{q}}_{b_k}^{c_0}}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^{c_0})$，根据上面的代价函数构造$\mathbf{A}\mathbf{x}=\mathbf{B}$，即
$${{\mathbf{J}}_{b_w}^{\gamma }}^T{\mathbf{J}}_{b_w}^{\gamma }\delta b_w=2{{\mathbf{J}}_{b_w}^{\gamma }}^T({{\hat{\gamma }}_{b_{k+1}}^{b_k}}^{-1}\otimes {{\mathbf{q}}_{b_k}^{c_0}}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^{c_0})$$
然后用LDLT分解求得偏置$\delta b_w$.代码如下

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;
    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();
        
        //R_ij = (R^c0_bk)^-1 * (R^c0_bk+1) 轉換爲四元數 q_ij = (q^c0_bk)^-1 * (q^c0_bk+1)
        Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
        //tmp_A = J_j_bw
        tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
        //tmp_b = 2 * ((r^bk_bk+1)^-1 * (q^c0_bk)^-1 * (q^c0_bk+1))_vec
        //      = 2 * ((r^bk_bk+1)^-1 * q_ij)_vec
        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
        //tmp_A * delta_bg = tmp_b
        A += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;
    }
    //LDLT方法
    delta_bg = A.ldlt().solve(b);
    ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());

    for (int i = 0; i <= WINDOW_SIZE; i++)
        Bgs[i] += delta_bg;

    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]);
    }
}

速度/重力和尺度初始化 LinearAligment()

该函数需要优化的变量有速度/重力加速度和尺度：
$${\mathbf{X}}_I^{3(n+1)+3+1}=[{\mathbf{v}}_{b_0}^{b_0},{\mathbf{v}}_{b_1}^{b_1},\dots ,{\mathbf{v}}_{b_n}^{b_n},{\mathbf{g}}^{c_0},s]$$
其中，${\mathbf{v}}_{b_k}^{b_k}$是第$k$帧图像在其IMU坐标系下的速度，${\mathbf{g}}^{c_0}$是在第0帧相机坐标系下的重力向量。
在IMU预积分中，有
$$\begin{gathered} {\mathbf{R}}_w^{b_k}{\mathbf{p}}_{b_{k+1}}^w={\mathbf{R}}_w^{b_k}({\mathbf{p}}_{b_k}^w+{\mathbf{v}}_{b_k}^w\Delta t_k-\frac{1}{2}{\mathbf{g}}^w\Delta t_k^2)+{\alpha }_{b_{k+1}}^{b_k} \\ {\mathbf{R}}_w^{b_k}{\mathbf{v}}_{b_{k+1}}^w={\mathbf{R}}_w^{b_k}({\mathbf{v}}_{b_k}^w-{\mathbf{g}}^w\Delta t_k+{\beta }_{b_{k+1}}^{b_k} \end{gathered}$$
将$c_0$替代$w$，有
$$\begin{gathered} {\alpha }_{b_{k+1}}^{b_k}={\mathbf{R}}_{c_0}^{b_k}(s{\mathbf{p}}_{b_{k+1}}^{c_0}-s{\mathbf{p}}_{b_k}^{c_0}+\frac{1}{2}{\mathbf{g}}^{c_0}\Delta t_k^2-{\mathbf{R}}_{b_k}^{c_0}{\mathbf{v}}_{b_k}^{b_k}\Delta t_k) \\ {\beta }_{b_{k+1}}^{b_k}={\mathbf{R}}_{c_0}^{b_k}({\mathbf{R}}_{b_{k+1}}^{c_0}{\mathbf{v}}_{b_{k+1}}^{b_{k+1}}+{\mathbf{g}}^{c_0}\Delta t_k-{\mathbf{R}}_{b_k}^{c_0}{\mathbf{v}}_{b_k}^{b_k}) \end{gathered}$$
进一步，有
$${\alpha }_{b_{k+1}}^{b_k}={\mathbf{R}}_{c_0}^{b_k}(s{\mathbf{p}}_{c_{k+1}}^{c_0}-{\mathbf{R}}_{b_{k+1}}^{c_0}{\mathbf{p}}_c^b-(s{\mathbf{p}}_{c_k}^{c_0}-{\mathbf{R}}_{b_k}^{c_0}{\mathbf{p}}_c^b)+\frac{1}{2}{\mathbf{g}}^{c_0}\Delta t_k^2-{\mathbf{R}}_{b_k}^{c_0}{\mathbf{v}}_{b_k}^{b_k}\Delta t_k)$$
根据待优化变量，整理上述方程，转换成$Hx=b$的形式，有
$${\mathbf{R}}_{c_0}^{b_k}({\mathbf{p}}_{c_{k+1}}^{c_0}-{\mathbf{p}}_{c_k}^{c_0})s-{\mathbf{v}}_{b_k}^{b_k}\Delta t_k+\frac{1}{2}{\mathbf{R}}_{c_0}^{b_k}\Delta t_k^2{\mathbf{g}}^{c_0}=\delta {\alpha }_{b_{k+1}}^{b_k}+{\mathbf{R}}_{c_0}^{b_k}{\mathbf{R}}_{b_{k+1}}^{c_0}\mathbf{p}{)}_c^b-{\mathbf{p}}_c^b$$
转换成矩阵形式：
$$\left[\begin{array}{cccc}-\mathbf{I}\Delta t_k& 0& \frac{1}{2}{\mathbf{R}}_{c_0}^{b_k}\Delta t_k^2& {\mathbf{R}}_{c_0}^{b_k}({\mathbf{p}}_{c_{k+1}}^{c_0}-{\mathbf{p}}_{c_k}^{c_0})\end{array}\right]\left[\begin{array}{c}{\mathbf{v}}_{b_k}^{b_k}\\ {\mathbf{v}}_{b_{k+1}}^{b_{k+1}}\\ {\mathbf{g}}^{c_0}\\ s\end{array}\right]={\hat{\alpha }}_{b_{k+1}}^{b_k}-{\mathbf{p}}_c^b+{\mathbf{R}}_{c_0}^{b_k}{\mathbf{R}}_{b_{k+1}}^{c_0}{\mathbf{p}}_c^b$$
同理，有
$$\left[\begin{array}{cccc}-\mathbf{I}& {\mathbf{R}}_{c_0}^{b_k}{\mathbf{R}}_{b_{k+1}}^{c_0}& {\mathbf{R}}_{c_0}^{b_k}\Delta t_k& 0\end{array}\right]\left[\begin{array}{c}{\mathbf{v}}_{b_k}^{b_k}\\ {\mathbf{v}}_{b_{k+1}}^{b_{k+1}}\\ {\mathbf{g}}^{c_0}\\ s\end{array}\right]={\hat{\beta }}_{b_{k+1}}^{b_k}$$
最后得到
$$\begin{gathered} {\mathbf{H}}^{6\times 10}{\mathbf{X}}_I^{10\times 1}={\mathbf{b}}^{6\times 1} \\ {\mathbf{H}}^T\mathbf{H}{\mathbf{X}}_I^{10\times 1}={\mathbf{H}}^T{\mathbf{b}}^{6\times 1} \end{gathered}$$
使用LDLT分解求解上式。代码中，MatrixXd tmp_A(6, 10)就是$\mathbf{H}$，VectorXd tmp_b(6,1)就是$\mathbf{b}$。

bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)

最后求得尺度$s$和重力加速度$\mathbf{g}$

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;
    }

重力细化RefineGravity()

考虑到上一步求得的g存在误差，一般认为重力矢量的模长是已知的，因此重力只剩下两个自由度，在切线空间上用两个变量重新参数化重力，将其表示为
$$\hat{\mathbf{g}}=\Vert \mathbf{g}\Vert {\overline{\hat{\mathbf{g}}}}^{c_0}+w_1{b}_1+w_2{b}_2=\Vert \mathbf{g}\Vert {\overline{\hat{\mathbf{g}}}}^{c_0}+{\mathbf{b}}^{\mathbf{T}}\mathbf{w}$$
将其带入优化求解公式，待优化变量和观测方程变为
$$\begin{gathered} \left[\begin{array}{c}{\mathbf{v}}_{b_k}^{b_k}\\ {\mathbf{v}}_{b_{k+1}}^{b_{k+1}}\\ {\mathbf{g}}^{c_0}\\ s\end{array}\right]\to \left[\begin{array}{c}{\mathbf{v}}_{b_k}^{b_k}\\ {\mathbf{v}}_{b_{k+1}}^{b_{k+1}}\\ {\mathbf{w}}^{c_0}\\ s\end{array}\right] \\ {\hat{\mathbf{z}}}_{b_{k+1}}^{b_k}=\left[\begin{array}{c}{\hat{\alpha }}_{b_{k+1}}^{b_k}-{\mathbf{p}}_c^b+{\mathbf{R}}_{c_0}^{b_k}{\mathbf{R}}_{b_{k+1}}^{c_0}{\mathbf{p}}_c^b-\frac{1}{2}{\mathbf{R}}_{c_0}^{b_k}\Delta t_k^2\Vert \mathbf{g}\Vert \cdot {\overline{\hat{\mathbf{g}}}}^{c_0}\\ {\beta }_{b_{k+1}}^{b_k}-{\mathbf{R}}_{c_0}^{b_k}\Delta t_k\Vert \mathbf{g}\Vert \cdot {\overline{\hat{\mathbf{g}}}}^{c_0}\end{array}\right] \end{gathered}$$
对应的函数为

void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)

函数中的MatrixXd lxly(3, 2)= TangentBasis(g0); 即对应$\mathbf{b}$,VectorXd dg = x.segment<2>(n_state - 3); 即对应$\mathbf{w}$，用于寻找参数$\mathbf{b}$的Algorithm 1对应的代码如下

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;
}

外参标定

VINS外参标定指的是对相机坐标系到IMU坐标系的变换矩阵进行在线标定与优化。

在参数配置文件yaml中，参数estimate_extrinsic反映了外参的情況：
1、等于0表示这外参不需要再优化；
2、等于1表示外参只是一個估计值，后续还需要将其作为初始值放入非线性优化中；
3、等于2表示不知道外参，需要进行标定，从代码estimator.cpp中的processImage()中的以下代码進入，主要是标定外参的旋转矩阵。

if(ESTIMATE_EXTRINSIC == 2)//如果沒有外參則進行標定
    {
        ROS_INFO("calibrating extrinsic param, rotation movement is needed");
        if (frame_count != 0)
        {
            //得到當前幀與前一幀之間歸一化特徵點
            vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
            Matrix3d calib_ric;
            //標定從camera到IMU之間的旋轉矩陣
            if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
            {
                ROS_WARN("initial extrinsic rotation calib success");
                ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
                ric[0] = calib_ric;
                RIC[0] = calib_ric;
                ESTIMATE_EXTRINSIC = 1;
            }
        }
    }

CalibrationExRotation()标定外参旋转矩阵

该函数的目的是标定外惨的旋转矩阵。下面介绍函数的内部操作。
1. solveRelativeR(corres)根据对极几何计算相邻帧见相机坐标系的旋转矩阵，这里的corres传入的是当前帧和其之前一帧的对应特征点对的归一化坐标。
1.1将corres中对应点的二维坐标分别存入ll和rr中

vector<cv::Point2f> ll, rr;
        for (int i = 0; i < int(corres.size()); i++)
        {
            ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
            rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
        }

1.2 根据对极几何求解这两帧的本质矩阵

cv::Mat E = cv::findFundamentalMat(ll, rr);

1.3 对本质矩阵进行svd分解得到4组Rt解

cv::Mat_<double> R1, R2, t1, t2;
decomposeE(E, R1, R2, t1, t2);

1.4 采用三角化对每组Rt进行验证，选择是正深度的Rt

double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2));
        double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2));
        cv::Mat_<double> ans_R_cv = ratio1 > ratio2 ? R1 : R2;

1.5 当前的R是上一帧到当前帧的变换矩阵，对其求转置变为当前帧想对于上一帧的姿态。

Matrix3d ans_R_eigen;
for (int i = 0; i < 3; i++)
   for (int j = 0; j < 3; j++)
       ans_R_eigen(j, i) = ans_R_cv(i, j);
return ans_R_eigen;

2. 获取相邻帧之间相机坐标系和IMU坐标系的旋转矩阵，存入vector中

    frame_count++;
    Rc.push_back(solveRelativeR(corres));//幀間cam的R，由對極幾何得到
    Rimu.push_back(delta_q_imu.toRotationMatrix());//幀間IMU的R，由IMU預積分得到
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);//每幀IMU相對於起始幀IMU的R

3. 求解相机到IMU的外参旋转矩阵，根据等式
$$\begin{gathered} {\mathbf{R}}_{c_{k+1}}^{b_k}={\mathbf{R}}_{b_{k+1}}^{b_k}\cdot {\mathbf{R}}_c^b={\mathbf{R}}_c^b\cdot {\mathbf{R}}_{b_{c+1}}^{c_k} \\ {\mathbf{q}}_{b_{k+1}}^{b_k}\cdot {\mathbf{q}}_c^b={\mathbf{q}}_c^b\cdot {\mathbf{q}}_{b_{c+1}}^{c_k} \end{gathered}$$
则可以得到
$$([{\mathbf{q}}_{b_{k+1}}^{b_k}{]}_L-[{\mathbf{q}}_{c_{k+1}}^{c_k}{]}_R){\mathbf{q}}_c^b={\mathbf{Q}}_{k+1}^k\cdot {\mathbf{q}}_c^b=0$$
将多帧之间的等式关联在一起构建超定方程$Ax=0$，对$A$进行svd分解，其中最小的奇异值对应的右奇异向量便为$x$的估计。对应的代码为

Eigen::MatrixXd A(frame_count * 4, 4);
    A.setZero();
    int sum_ok = 0;
    for (int i = 1; i <= frame_count; i++)
    {
        Quaterniond r1(Rc[i]);
        Quaterniond r2(Rc_g[i]);
        double angular_distance = 180 / M_PI * r1.angularDistance(r2);
        ROS_DEBUG(
            "%d %f", i, angular_distance);
        //huber核函數做加權
        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        ++sum_ok;
        Matrix4d L, R;
        //L R 分別爲左乘和右乘矩陣
        double w = Quaterniond(Rc[i]).w();
        Vector3d q = Quaterniond(Rc[i]).vec();
        L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
        L.block<3, 1>(0, 3) = q;
        L.block<1, 3>(3, 0) = -q.transpose();
        L(3, 3) = w;

        Quaterniond R_ij(Rimu[i]);
        w = R_ij.w();
        q = R_ij.vec();
        R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
        R.block<3, 1>(0, 3) = q;
        R.block<1, 3>(3, 0) = -q.transpose();
        R(3, 3) = w;

        A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
    }

    //svd分解中最小奇異值對應的右奇異向量作爲旋轉四元數
    JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
    Matrix<double, 4, 1> x = svd.matrixV().col(3);
    Quaterniond estimated_R(x);
    ric = estimated_R.toRotationMatrix().inverse();

4. 至少迭代计算了WINDOW_SIZE次，其R的奇异值大于0。25才认为标定成功。

   Vector3d ric_cov;
    ric_cov = svd.singularValues().tail<3>();
    if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
    {
        calib_ric_result = ric;
        return true;
    }
    else
        return false;

在初步标定完外参的旋转矩阵后，后续会对旋转矩阵与平移向量进行继续优化。