前言

本文主要介绍VINS状态估计器模块（estimator）初始化环节中视觉惯性对齐求解陀螺仪偏置、尺度、重力加速度、每帧速度以及相机到IMU的外参估计，其中前半部分对应论文第五章（V. ESTIMATOR INITIALIZATION B. Visual-Inertial Alignment），后半部分参考了沈老师组的之前的论文。

总的来说，视觉惯性对齐主要包括以下流程：

1、若旋转外参数 q b c q_{bc} qbc未知，则先估计旋转外参

2、利用旋转约束估计陀螺仪bias

3、利用平移约束估计重力方向，速度，以及尺度初始值

4、对重力向量 g c 0 g^{c_0} gc0进一步优化

5、求解世界坐标系w和初始相机坐标系 c 0 c_0 c0之间的旋转矩阵，并讲轨迹对齐到世界坐标系

视觉惯性对齐

首先我们先推导论文式(14)，所有帧的位姿 ( R c k c 0 , q c k c 0 ) (R_{c_k}^{c_0},q_{c_k}^{c_0}) (Rckc0,qckc0)表示相对于第一帧相机坐标系(·)c0。相机到IMU的外参为 ( R c b , q c b ) (R_c^b,q_c^b) (Rcb,qcb)，得到姿态从相机坐标系转换到IMU坐标系的关系。

T c k c 0 = T b k c 0 T c b T^{c_0}_{c_k}=T^{c_0}_{b_k}T^{b}_{c} Tckc0=Tbkc0Tcb

将T展开有成R与p有：

[ R c k c 0 s p c k c 0 0 1 ] = [ R b k c 0 s p b k c 0 0 1 ] [ R c b p c b 0 1 ] \left[\begin{array}{cccc} R^{c_0}_{c_k} &sp^{c_0}_{c_k}\\0&1 \end{array}\right]=\left[\begin{array}{cccc} R^{c_0}_{b_k} &sp^{c_0}_{b_k}\\0&1 \end{array}\right] \left[\begin{array}{cccc} R^{b}_{c} &p^{b}_{c}\\0&1 \end{array}\right] [Rckc00spckc01]=[Rbkc00spbkc01][Rcb0pcb1]

左侧矩阵的两项写开：

R c k c 0 = R b k c 0 R c b ⇒ R b k c 0 = R c k c 0 ( R c b ) − 1 R^{c_0}_{c_k}=R^{c_0}_{b_k}R^{b}_{c} \Rightarrow R^{c_0}_{b_k}=R^{c_0}_{c_k}(R^{b}_{c})^{-1} Rckc0=Rbkc0Rcb⇒Rbkc0=Rckc0(Rcb)−1

s p c k c 0 = R b k c 0 p c b + s p b k c 0 ⇒ s p b k c 0 = s p c k c 0 − R b k c 0 p c b sp^{c_0}_{c_k}=R^{c_0}_{b_k} p^{b}_{c} +sp^{c_0}_{b_k}\Rightarrow sp^{c_0}_{b_k}=sp^{c_0}_{c_k}-R^{c_0}_{b_k} p^{b}_{c} spckc0=Rbkc0pcb+spbkc0⇒spbkc0=spckc0−Rbkc0pcb

对于VINS初始化中视觉惯性对齐之外的部分已在博客VINS-Mono代码解读——视觉惯性联合初始化中详细介绍了。这里就直接从VisualIMUAlignment()函数开始，结合相关数学推导进行解读。这里的代码均位于vins_estimator/src/initial/initial_aligment.cpp/.h。

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 bk和 b k + 1 b_{k+1} bk+1，已经从视觉SFM中得到了旋转 q b k c 0 q^{c_0}_{b_k} qbkc0和 q b k + 1 c 0 q^{c_0}_{b_{k+1}} qbk+1c0，从IMU预积分中得到了相邻帧旋转 γ ^ b k + 1 b k \hat \gamma^{b_k}_{b_{k+1}} γ^bk+1bk。根据约束方程，联立所有相邻帧，最小化代价函数（论文式(15)）：

min ⁡ δ b w ∑ k ∈ B ∥ q b k + 1 c 0 − 1 ⊗ q b k c 0 ⊗ γ b k + 1 b k ∥ 2 \mathop {\min }\limits_{\delta b_w} \sum\nolimits_{k\in \mathcal B} \left\|{q^{c_0}_{b_{k+1}}}^{-1}\otimes q^{c_0}_{b_{k}}\otimes \gamma^{b_k}_{b_{k+1}} \right\|^2 δbwmin∑k∈B∥∥∥qbk+1c0−1⊗qbkc0⊗γbk+1bk∥∥∥2

其中对陀螺仪偏置求IMU预积分项线性化，有：

γ b k + 1 b k ≈ γ ^ b k + 1 b k ⊗ [ 1 1 2 J b w γ δ b w ] \gamma^{b_k}_{b_{k+1}}\approx \hat \gamma^{b_k}_{b_{k+1}} \otimes \left[\begin{array}{cccc} 1 \\ \frac{1}{2}J^{\gamma}_{b_w}\delta b_w \end{array}\right] γbk+1bk≈γ^bk+1bk⊗[121Jbwγδbw]

在具体实现的时候，因为上述约束方程为：

q b k + 1 c 0 − 1 ⊗ q b k c 0 ⊗ γ b k + 1 b k = [ 1 0 ] {q^{c_0}_{b_{k+1}}}^{-1}\otimes q^{c_0}_{b_{k}}\otimes \gamma^{b_k}_{b_{k+1}}= \left[\begin{array}{cccc} 1 \\ 0\end{array}\right] qbk+1c0−1⊗qbkc0⊗γbk+1bk=[10]

有：

γ b k + 1 b k = q b k c 0 − 1 ⊗ q b k + 1 c 0 ⊗ [ 1 0 ] \gamma^{b_k}_{b_{k+1}}= {q^{c_0}_{b_{k}}}^{-1}\otimes q^{c_0}_{b_{k+1}}\otimes \left[\begin{array}{cccc} 1 \\ 0\end{array}\right] γbk+1bk=qbkc0−1⊗qbk+1c0⊗[10]

代入 δ b w \delta b_w δbw得：

γ ^ b k + 1 b k ⊗ [ 1 1 2 J b w γ δ b w ] = q b k c 0 − 1 ⊗ q b k + 1 c 0 ⊗ [ 1 0 ] \hat \gamma^{b_k}_{b_{k+1}} \otimes \left[\begin{array}{cccc} 1 \\ \frac{1}{2}J^{\gamma}_{b_w}\delta b_w \end{array}\right] ={q^{c_0}_{b_{k}}}^{-1}\otimes q^{c_0}_{b_{k+1}}\otimes \left[\begin{array}{cccc} 1 \\ 0\end{array}\right] γ^bk+1bk⊗[121Jbwγδbw]=qbkc0−1⊗qbk+1c0⊗[10]

只考虑虚部，有：

J b w γ δ b w = 2 ( γ ^ b k + 1 b k − 1 ⊗ q b k c 0 − 1 ⊗ q b k + 1 c 0 ) v e c J^{\gamma}_{b_w}\delta b_w = 2({\hat \gamma^{b_k}_{b_{k+1}}}^{-1} \otimes{q^{c_0}_{b_{k}}}^{-1}\otimes q^{c_0}_{b_{k+1}})_{vec} Jbwγδbw=2(γ^bk+1bk−1⊗qbkc0−1⊗qbk+1c0)vec

两侧乘以 J b w γ T {J^\gamma_{b_w}}^T JbwγT，用LDLT分解求得 δ b w \delta b_w δbw。

在代码中，Quaterniond q_ij即 q b k + 1 b k = q b k c 0 − 1 ⊗ q b k + 1 c 0 q^{b_k}_{b_{k+1}}={q^{c_0}_{b_k}}^{-1} \otimes q^{c_0}_{b_{k+1}} qbk+1bk=qbkc0−1⊗qbk+1c0

tmp_A即 J b w γ J^\gamma_{b_w} Jbwγ

tmp_B即 2 ( r ^ b k + 1 b k − 1 ⊗ q b k + 1 b k ) v e c = 2 ( r ^ b k + 1 b k − 1 ⊗ q b k c 0 − 1 ⊗ q b k + 1 c 0 ) v e c 2 ({\hat r^{b_k}_{b_{k+1}}}^{-1} \otimes q^{b_k}_{b_{k+1}})_{vec}=2 ({\hat r^{b_k}_{b_{k+1}}}^{-1} \otimes {q^{c_0}_{b_k}}^{-1} \otimes q^{c_0}_{b_{k+1}})_{vec} 2(r^bk+1bk−1⊗qbk+1bk)vec=2(r^bk+1bk−1⊗qbkc0−1⊗qbk+1c0)vec

根据上面的代价函数构造Ax = B即

J b w γ T J b w γ δ b w = 2 J b w γ T ( r ^ b k + 1 b k − 1 ⊗ q b k c 0 − 1 ⊗ q b k + 1 c 0 ) v e c {J^\gamma_{b_w}}^T J^\gamma_{b_w} \delta b_w=2 {J^\gamma_{b_w}}^T({\hat r^{b_k}_{b_{k+1}}}^{-1} \otimes {q^{c_0}_{b_k}}^{-1} \otimes q^{c_0}_{b_{k+1}})_{vec} JbwγTJbwγδbw=2JbwγT(r^bk+1bk−1⊗qbkc0−1⊗qbk+1c0)vec

然后采用LDLT分解求得 δ b w \delta b_w δbw。

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

速度、重力和尺度初始化 LinearAlignment()

该函数需要优化的变量有速度、重力加速度和尺度（论文式(16)）：

X I 3 ( n + 1 ) + 3 + 1 = [ v b 0 b 0 , v b 1 b 1 , . . . , v b n b n , g c 0 , s ] \mathcal X^{3(n+1)+3+1}_I=[v^{b_0}_{b_0},v^{b_1}_{b_1},...,v^{b_n}_{b_n},g^{c_0},s] XI3(n+1)+3+1=[vb0b0,vb1b1,...,vbnbn,gc0,s]

其中， v b k b k v^{b_k}_{b_k} vbkbk是第k帧图像在其IMU坐标系下的速度， g c 0 g^{c_0} gc0是在第0帧相机坐标系下的重力向量。

在IMU预积分中我们已经推导过论文式(5)：

R w b k p b k + 1 w = R w b k ( p b k w + v b k w Δ t k − 1 2 g w Δ t k 2 ) + α b k + 1 b k R^{b_k}_w p^w_{b_{k+1}}=R^{b_k}_w (p^w_{b_k}+v^w_{b_k} \Delta t_k-\frac{1}{2} g^w\Delta t_k^2)+ \alpha^{b_k}_{b_{k+1}} Rwbkpbk+1w=Rwbk(pbkw+vbkwΔtk−21gwΔtk2)+αbk+1bk

R w b k v b k + 1 w = R w b k ( v b k w − g w Δ t k ) + β b k + 1 b k R^{b_k}_w v^w_{b_{k+1}}=R^{b_k}_w (v^w_{b_k}- g^w\Delta t_k)+\beta^{b_k}_{b_{k+1}} Rwbkvbk+1w=Rwbk(vbkw−gwΔtk)+βbk+1bk

将 c 0 c_0 c0替代 w w w可以写成：

α b k + 1 b k = R c 0 b k ( s p b k + 1 c 0 − s p b k c 0 + 1 2 g c 0 Δ t k 2 − R b k c 0 v b k b k Δ t k ) \alpha^{b_k}_{b_{k+1}}=R^{b_k}_{c_0}(sp^{c_0}_{b_{k+1}}-sp^{c_0}_{b_{k}}+\frac{1}{2} g^{c_0}\Delta t_k^2-R^{c_0}_{b_k}v^{b_k}_{b_k} \Delta t_k) αbk+1bk=Rc0bk(spbk+1c0−spbkc0+21gc0Δtk2−Rbkc0vbkbkΔtk)

β b k + 1 b k = R c 0 b k ( R b k + 1 c 0 v b k + 1 b k + 1 + g c 0 Δ t k − R b k c 0 v b k b k ) \beta^{b_k}_{b_{k+1}}=R^{b_k}_{c_0}(R^{c_0}_{b_{k+1}}v^{b_{k+1}}_{b_{k+1}}+g^{c_0}\Delta t_k-R^{c_0}_{b_k}v^{b_k}_{b_k}) βbk+1bk=Rc0bk(Rbk+1c0vbk+1bk+1+gc0Δtk−Rbkc0vbkbk)

将论文式(14)带入 α b k + 1 b k \alpha^{b_k}_{b_{k+1}} αbk+1bk有：

δ α b k + 1 b k = α b k + 1 b k − R c 0 b k ( s p b k + 1 c 0 − s p b k c 0 + 1 2 g c 0 Δ t k 2 − R b k c 0 v b k b k Δ t k ) \delta \alpha^{b_k}_{b_{k+1}}=\alpha^{b_k}_{b_{k+1}}-R^{b_k}_{c_0}(sp^{c_0}_{b_{k+1}}-sp^{c_0}_{b_{k}}+\frac{1}{2} g^{c_0}\Delta t_k^2-R^{c_0}_{b_k}v^{b_k}_{b_k} \Delta t_k) δαbk+1bk=αbk+1bk−Rc0bk(spbk+1c0−spbkc0+21gc0Δtk2−Rbkc0vbkbkΔtk)

= α b k + 1 b k − R c 0 b k ( s p c k + 1 c 0 − R b k + 1 c 0 p c b − ( s p c k c 0 − R b k c 0 p c b ) + 1 2 g c 0 Δ t k 2 − R b k c 0 v b k b k Δ t k ) =\alpha^{b_k}_{b_{k+1}}-R^{b_k}_{c_0}(sp^{c_0}_{c_{k+1}}-R^{c_0}_{b_{k+1}} p^{b}_{c}-(sp^{c_0}_{c_k}-R^{c_0}_{b_k} p^{b}_{c})+\frac{1}{2} g^{c_0}\Delta t_k^2-R^{c_0}_{b_k}v^{b_k}_{b_k} \Delta t_k) =αbk+1bk−Rc0bk(spck+1c0−Rbk+1c0pcb−(spckc0−Rbkc0pcb)+21gc0Δtk2−Rbkc0vbkbkΔtk)

= α b k + 1 b k − R c 0 b k s ( p c k + 1 c 0 − p c k c 0 ) + R c 0 b k R b k + 1 c 0 p c b − p c b + v b k b k Δ t k − 1 2 R c 0 b k g c 0 Δ t k 2 = 0 3 × 1 =\alpha^{b_k}_{b_{k+1}}-R^{b_k}_{c_0}s(p^{c_0}_{c_{k+1}}-p^{c_0}_{c_k})+R^{b_k}_{c_0}R^{c_0}_{b_{k+1}}p^b_c-p^b_c+v^{b_k}_{b_k}\Delta t_k-\frac{1}{2}R^{b_k}_{c_0} g^{c_0}\Delta t_k^2=0_{3\times1} =αbk+1bk−Rc0bks(pck+1c0−pckc0)+Rc0bkRbk+1c0pcb−pcb+vbkbkΔtk−21Rc0bkgc0Δtk2=03×1

这里 v b k b k , s , g c 0 v^{b_k}_{b_k},s,g^{c_0} vbkbk,s,gc0是待优化变量，将其转换成 H x = b Hx=b Hx=b的形式：

R c 0 b k ( p c k + 1 c 0 − p c k c 0 ) s − v b k b k Δ t k + 1 2 R c 0 b k Δ t k 2 g c 0 = α b k + 1 b k + R c 0 b k R b k + 1 c 0 p c b − p c b R^{b_k}_{c_0}(p^{c_0}_{c_{k+1}}-p^{c_0}_{c_k})s-v^{b_k}_{b_k}\Delta t_k+\frac{1}{2}R^{b_k}_{c_0}\Delta t_k^2 g^{c_0}= \alpha^{b_k}_{b_{k+1}}+R^{b_k}_{c_0}R^{c_0}_{b_{k+1}}p^b_c-p^b_c Rc0bk(pck+1c0−pckc0)s−vbkbkΔtk+21Rc0bkΔtk2gc0=αbk+1bk+Rc0bkRbk+1c0pcb−pcb

写成矩阵形式：

[ − I Δ t k 0 1 2 R c 0 b k Δ t k 2 R c 0 b k ( p c k + 1 c 0 − p c k c 0 ) ] [ v b k b k v b k + 1 b k + 1 g c 0 s ] = α b k + 1 b k + R c 0 b k R b k + 1 c 0 p c b − p c b \left[\begin{array}{cccc} -I\Delta t_k &0&\frac{1}{2}R^{b_k}_{c_0}\Delta t_k^2 & R^{b_k}_{c_0}(p^{c_0}_{c_{k+1}}-p^{c_0}_{c_k})\end{array}\right] \left[\begin{array}{cccc} v^{b_k}_{b_k}\\v^{b_{k+1}}_{b_{k+1}}\\g^{c_0}\\s \end{array}\right] = \alpha^{b_k}_{b_{k+1}}+R^{b_k}_{c_0}R^{c_0}_{b_{k+1}}p^b_c-p^b_c [−IΔtk021Rc0bkΔtk2Rc0bk(pck+1c0−pckc0)]⎣⎢⎢⎡vbkbkvbk+1bk+1gc0s⎦⎥⎥⎤=αbk+1bk+Rc0bkRbk+1c0pcb−pcb

对于 β b k + 1 b k \beta^{b_k}_{b_{k+1}} βbk+1bk有：

δ β b k + 1 b k = β b k + 1 b k − R c 0 b k ( R b k + 1 c 0 v b k + 1 b k + 1 + g c 0 Δ t k − R b k c 0 v b k b k ) = 0 3 × 1 \delta \beta^{b_k}_{b_{k+1}}=\beta^{b_k}_{b_{k+1}}-R^{b_k}_{c_0}(R^{c_0}_{b_{k+1}}v^{b_{k+1}}_{b_{k+1}}+g^{c_0}\Delta t_k-R^{c_0}_{b_k}v^{b_k}_{b_k})=0_{3\times1} δβbk+1bk=βbk+1bk−Rc0bk(Rbk+1c0vbk+1bk+1+gc0Δtk−Rbkc0vbkbk)=03×1

R c 0 b k R b k + 1 c 0 v b k + 1 b k + 1 + R c 0 b k Δ t k g c 0 − R c 0 b k R b k c 0 v b k b k = β b k + 1 b k R^{b_k}_{c_0}R^{c_0}_{b_{k+1}}v^{b_{k+1}}_{b_{k+1}}+R^{b_k}_{c_0}\Delta t_kg^{c_0}-R^{b_k}_{c_0}R^{c_0}_{b_k}v^{b_k}_{b_k}=\beta^{b_k}_{b_{k+1}} Rc0bkRbk+1c0vbk+1bk+1+Rc0bkΔtkgc0−Rc0bkRbkc0vbkbk=βbk+1bk

写成矩阵形式：

[ − I R c 0 b k R b k + 1 c 0 R c 0 b k Δ t k 0 ] [ v b k b k v b k + 1 b k + 1 g c 0 s ] = β b k + 1 b k \left[\begin{array}{cccc} -I&R^{b_k}_{c_0}R^{c_0}_{b_{k+1}}&R^{b_k}_{c_0}\Delta t_k& 0\end{array}\right] \left[\begin{array}{cccc} v^{b_k}_{b_k}\\v^{b_{k+1}}_{b_{k+1}}\\g^{c_0}\\s \end{array}\right] = \beta^{b_k}_{b_{k+1}} [−IRc0bkRbk+1c0Rc0bkΔtk0]⎣⎢⎢⎡vbkbkvbk+1bk+1gc0s⎦⎥⎥⎤=βbk+1bk

由此得到论文式(18)(19)：

[ − I Δ t k 0 1 2 R c 0 b k Δ t k 2 R c 0 b k ( p c k + 1 c 0 − p c k c 0 ) − I R c 0 b k R b k + 1 c 0 R c 0 b k Δ t k 0 ] [ v b k b k v b k + 1 b k + 1 g c 0 s ] = [ α b k + 1 b k + R c 0 b k R b k + 1 c 0 p c b − p c b β b k + 1 b k ] \left[\begin{array}{cccc} -I\Delta t_k &0&\frac{1}{2}R^{b_k}_{c_0}\Delta t_k^2 & R^{b_k}_{c_0}(p^{c_0}_{c_{k+1}}-p^{c_0}_{c_k})\\-I&R^{b_k}_{c_0}R^{c_0}_{b_{k+1}}&R^{b_k}_{c_0}\Delta t_k& 0\end{array}\right] \left[\begin{array}{cccc} v^{b_k}_{b_k}\\v^{b_{k+1}}_{b_{k+1}}\\g^{c_0}\\s \end{array}\right] =\left[\begin{array}{cccc} \alpha^{b_k}_{b_{k+1}}+R^{b_k}_{c_0}R^{c_0}_{b_{k+1}}p^b_c-p^b_c\\ \beta^{b_k}_{b_{k+1}} \end{array}\right] [−IΔtk−I0Rc0bkRbk+1c021Rc0bkΔtk2Rc0bkΔtkRc0bk(pck+1c0−pckc0)0]⎣⎢⎢⎡vbkbkvbk+1bk+1gc0s⎦⎥⎥⎤=[αbk+1bk+Rc0bkRbk+1c0pcb−pcbβbk+1bk]

即 H 6 × 10 X I 10 × 1 = b 6 × 1 H^{6\times10}\mathcal X^{10\times1}_I=b^{6\times1} H6×10XI10×1=b6×1，使用LDLT分解求解：

H T H X I 10 × 1 = H T b H^TH\mathcal X^{10\times1}_I=H^Tb HTHXI10×1=HTb

对应代码实现在以下函数中，因为内容和上述理论一致就不放代码了。里面的MatrixXd tmp_A(6, 10)就是H，VectorXd tmp_b(6,1)就是b

最后求得尺度s和重力加速度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存在误差，一般认为重力矢量的模是已知的，因此重力向量只剩两个自由度，在切线空间上用两个变量重新参数化重力，将其表示为

g ^ = ∣ ∣ g ∣ ∣ g ^ ˉ + w 1 b 1 + w 2 b 2 = ∣ ∣ g ∣ ∣ g ^ ˉ + b ⃗ 3 × 2 w 2 × 1 \hat g=||g||\bar{\hat g}+w_1b_1+w_2b_2=||g||\bar{\hat g}+\vec b^{3\times2}w^{2\times1} g^=∣∣g∣∣g^ˉ+w1b1+w2b2=∣∣g∣∣g^ˉ+b

3×2w2×1

其中 w 2 × 1 = [ w 1 , w 2 ] T , b ⃗ 3 × 2 = [ b 1 , b 2 ] w^{2\times1}=[w_1,w_2]^T,\vec b^{3\times2}=[b_1,b_2] w2×1=[w1,w2]T,b

3×2=[b1,b2]

g ^ ˉ \bar{\hat g} g^ˉ为上一步求得的重力方向，, b 1 、 b 2 b_1、b_2 b1、b2在 g ^ \hat g g^的正切平面上且正交。 b 1 b_1 b1与 b 2 b_2 b2的设置为：

VINS-Mono理论学习——视觉惯性联合初始化与外参标定前言视觉惯性对齐外参标定

将其带入论文式(18)(19)中，有：

[ − I Δ t k 0 1 2 R c 0 b k Δ t k 2 b ⃗ R c 0 b k ( p c k + 1 c 0 − p c k c 0 ) − I R c 0 b k R b k + 1 c 0 R c 0 b k Δ t k b ⃗ 0 ] [ v b k b k v b k + 1 b k + 1 w s ] = [ δ α b k + 1 b k + R c 0 b k R b k + 1 c 0 p c b − p c b − 1 2 R c 0 b k Δ t k 2 ∣ ∣ g ∣ ∣ g ^ ˉ β b k + 1 b k − R c 0 b k Δ t k ∣ ∣ g ∣ ∣ g ^ ˉ ] \left[\begin{array}{cccc} -I\Delta t_k &0&\frac{1}{2}R^{b_k}_{c_0}\Delta t_k^2 \vec b & R^{b_k}_{c_0}(p^{c_0}_{c_{k+1}}-p^{c_0}_{c_k})\\-I&R^{b_k}_{c_0}R^{c_0}_{b_{k+1}}&R^{b_k}_{c_0}\Delta t_k \vec b& 0\end{array}\right] \left[\begin{array}{cccc} v^{b_k}_{b_k}\\v^{b_{k+1}}_{b_{k+1}}\\w\\s \end{array}\right] =\left[\begin{array}{cccc} \delta \alpha^{b_k}_{b_{k+1}}+R^{b_k}_{c_0}R^{c_0}_{b_{k+1}}p^b_c-p^b_c-\frac{1}{2}R^{b_k}_{c_0}\Delta t_k^2||g||\bar{\hat g}\\ \beta^{b_k}_{b_{k+1}}-R^{b_k}_{c_0}\Delta t_k||g||\bar{\hat g} \end{array}\right] [−IΔtk−I0Rc0bkRbk+1c021Rc0bkΔtk2b

Rc0bkΔtkb

Rc0bk(pck+1c0−pckc0)0]⎣⎢⎢⎡vbkbkvbk+1bk+1ws⎦⎥⎥⎤=[δαbk+1bk+Rc0bkRbk+1c0pcb−pcb−21Rc0bkΔtk2∣∣g∣∣g^ˉβbk+1bk−Rc0bkΔtk∣∣g∣∣g^ˉ]

即 H 6 × 9 X I 9 × 1 = b 6 × 1 H^{6\times9}\mathcal X^{9\times 1}_I=b^{6\times1} H6×9XI9×1=b6×1。使用LDLT分解求解：

H T H X I 9 × 1 = H T b H^TH\mathcal X^{9\times1}_I=H^Tb HTHXI9×1=HTb

在代码实现中，以下函数用于重力细化，其流程基本对应上文推导，方程迭代求解4次：

函数中的MatrixXd lxly(3, 2)= TangentBasis(g0); 即对应 b ⃗ 3 × 2 = [ b 1 , b 2 ] \vec b^{3\times2}=[b_1,b_2] b

3×2=[b1,b2]，

VectorXd dg = x.segment<2>(n_state - 3); 即对应 w 2 × 1 = [ w 1 , w 2 ] T w^{2\times1}=[w_1,w_2]^T w2×1=[w1,w2]T

以下函数对应论文中Algorithm 1，用于在半径为g的半球找到切面的一对正交基。

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

InitialEXRotation类

该类位于vins_estimator/src/initial/initial_ex_rotation.cpp/.h中，用于标定外参旋转矩阵。首先简要介绍一下InitialEXRotation类的成员：

class InitialEXRotation
{
public:
	InitialEXRotation();//构造函数
	//标定外参旋转矩阵
    bool CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result);
private:
	//求解帧间cam坐标系的旋转矩阵
	Matrix3d solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres);
	//三角化验证Rt
    double testTriangulation(const vector<cv::Point2f> &l,
                             const vector<cv::Point2f> &r,
                             cv::Mat_<double> R, cv::Mat_<double> t);
    //本质矩阵SVD分解计算4组Rt值
    void decomposeE(cv::Mat E,
                    cv::Mat_<double> &R1, cv::Mat_<double> &R2,
                    cv::Mat_<double> &t1, cv::Mat_<double> &t2);
                    
    int frame_count;//帧计数器
    vector< Matrix3d > Rc;//帧间cam的R，由对极几何得到
    vector< Matrix3d > Rimu;//帧间IMU的R，由IMU预积分得到
    vector< Matrix3d > Rc_g;//每帧IMU相对于起始帧IMU的R
    Matrix3d ric;//cam到IMU的外参
};

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、根据对极几何求解这两帧的本质矩阵

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的外参旋转矩阵，根据等式：

R c k + 1 b k = R b k + 1 b k ⋅ R c b = R c b ⋅ R c k + 1 c k R_{c_{k+1}}^{b_k}=R_{b_{k+1}}^{b_k} \cdot R_c^b = R_c^b \cdot R_{c_{k+1}}^{c_k} Rck+1bk=Rbk+1bk⋅Rcb=Rcb⋅Rck+1ck

其中 R b k + 1 b k R_{b_{k+1}}^{b_k} Rbk+1bk为IMU坐标系之间的旋转矩阵， R c k + 1 c k R_{c_{k+1}}^{c_k} Rck+1ck为相机坐标系之间的旋转矩阵， R c b R_c^b Rcb为从相机到IMU的旋转矩阵。

用四元素重写：

q b k + 1 b k ⊗ q c b = q c b ⊗ q c k + 1 c k q_{b_{k+1}}^{b_k} \otimes q_c^b = q_c^b \otimes q_{c_{k+1}}^{c_k} qbk+1bk⊗qcb=qcb⊗qck+1ck

[ Q 1 ( q b k + 1 b k ) − Q 2 ( q c k + 1 c k ) ] ⋅ q c b = Q c b ⋅ q c b = 0 [\mathcal{Q_1}(q_{b_{k+1}}^{b_k})-\mathcal{Q_2}(q_{c_{k+1}}^{c_k})]\cdot q_c^b=Q^b_c\cdot q_c^b=0 [Q1(qbk+1bk)−Q2(qck+1ck)]⋅qcb=Qcb⋅qcb=0

将多个帧之间的等式关系一起构建超定方程 A x = 0 Ax=0 Ax=0。对A进行svd分解，其中最小奇异值对应的右奇异向量便为结果x，即旋转四元数 q c b q_c^b qcb。

在代码中，L为相机旋转四元数的左乘矩阵，R为IMU旋转四元数的右乘矩阵，因此其实构建的是：

q b c ⊗ q b k + 1 b k = q c k + 1 c k ⊗ q b c q_b^c \otimes q_{b_{k+1}}^{b_k} =q_{c_{k+1}}^{c_k} \otimes q_b^c qbc⊗qbk+1bk=qck+1ck⊗qbc

所求的x是 q b c q_b^c qbc，在最后需要转换成旋转矩阵并求逆。

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;

在初步标定完外参的旋转矩阵后，对旋转矩阵与平移向量的优化将在非线性优化中一起介绍。

VINS-Mono理论学习——视觉惯性联合初始化与外参标定前言视觉惯性对齐外参标定

前言

视觉惯性对齐

陀螺仪偏置标定 solveGyroscopeBias()

速度、重力和尺度初始化 LinearAlignment()

重力细化 RefineGravity()

外参标定

InitialEXRotation类

CalibrationExRotation() 标定外参旋转矩阵

继续阅读

使用tkinter 实现一个猜数字游戏

记一次优化JPA批量存储数据

CLR线程池的工作者线程

蓝桥杯嵌入式1、按键、流水灯

ZooKeeper ： Curator框架之共享计数器DistributedAtomicLong

Java网络编程-Socket简介

SPOJ LCS Longest Common Substring

GoFrame系列：5、项目配置、服务路由及运行方式

C++学习------类的五种构造函数

Spring Framework 源码阅读（五）：BeanFactoryPostProcessor

Spring Boot：延迟初始化与自定义横幅

HDU 5651 xiaoxin juju needs help

羊了个羊小程序通关脚本

Python - 统计字母个数

GMS：基于网格运动统计的快速极度鲁棒的特征匹配摘要1 简介2 本文的方法3 应用于快速记分的网格框架4 实验 5 结论

常见的error 1236 报错