VINS-Mono理論學習——視覺慣性聯合初始化與外參標定

原創 Manii 2020-05-30 19:31

前言

本文主要介紹VINS狀態估計器模塊（estimator）初始化環節中視覺慣性對齊求解陀螺儀偏置、尺度、重力加速度、每幀速度以及相機到IMU的外參估計，其中前半部分對應論文第五章（V. ESTIMATOR INITIALIZATION B. Visual-Inertial Alignment），後半部分參考了沈老師組的之前的論文。

總的來說，視覺慣性對齊主要包括以下流程：
1、若旋轉外參數qbcq_{bc}未知，則先估計旋轉外參
2、利用旋轉約束估計陀螺儀bias
3、利用平移約束估計重力方向，速度，以及尺度初始值
4、對重力向量gc0g^{c_0}進一步優化
5、求解世界座標系w和初始相機座標系c0c_0之間的旋轉矩陣，並講軌跡對齊到世界座標系

視覺慣性對齊

首先我們先推導論文式(14)，所有幀的位姿(Rckc0,qckc0)(R_{c_k}^{c_0},q_{c_k}^{c_0})表示相對於第一幀相機座標系(·)c0。相機到IMU的外參爲(Rcb,qcb)(R_c^b,q_c^b)，得到姿態從相機座標系轉換到IMU座標系的關係。
Tckc0=Tbkc0TcbT^{c_0}_{c_k}=T^{c_0}_{b_k}T^{b}_{c}
將T展開有成R與p有：
[Rckc0spckc001]=[Rbkc0spbkc001][Rcbpcb01]\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]
左側矩陣的兩項寫開：
Rckc0=Rbkc0Rcb⇒Rbkc0=Rckc0(Rcb)−1R^{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}
spckc0=Rbkc0pcb+spbkc0⇒spbkc0=spckc0−Rbkc0pcbsp^{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}
對於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()

對於窗口中得連續兩幀bkb_k和bk+1b_{k+1}，已經從視覺SFM中得到了旋轉qbkc0q^{c_0}_{b_k}和qbk+1c0q^{c_0}_{b_{k+1}}，從IMU預積分中得到了相鄰幀旋轉γ^bk+1bk\hat \gamma^{b_k}_{b_{k+1}}。根據約束方程，聯立所有相鄰幀，最小化代價函數（論文式(15)）：
min⁡δbw∑k∈B∥qbk+1c0−1⊗qbkc0⊗γbk+1bk∥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
其中對陀螺儀偏置求IMU預積分項線性化，有：
γbk+1bk≈γ^bk+1bk⊗[112Jbwγδbw]\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]

在具體實現的時候，因爲上述約束方程爲：
qbk+1c0−1⊗qbkc0⊗γbk+1bk=[10]{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]
有：
γbk+1bk=qbkc0−1⊗qbk+1c0⊗[10] \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]
代入δbw\delta b_w得：
γ^bk+1bk⊗[112Jbwγδbw]=qbkc0−1⊗qbk+1c0⊗[10] \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]
只考慮虛部，有：
Jbwγδbw=2(γ^bk+1bk−1⊗qbkc0−1⊗qbk+1c0)vec 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γT{J^\gamma_{b_w}}^T，用LDLT分解求得δbw\delta b_w。

在代碼中，Quaterniond q_ij即qbk+1bk=qbkc0−1⊗qbk+1c0q^{b_k}_{b_{k+1}}={q^{c_0}_{b_k}}^{-1} \otimes q^{c_0}_{b_{k+1}}
tmp_A即JbwγJ^\gamma_{b_w}
tmp_B即2(r^bk+1bk−1⊗qbk+1bk)vec=2(r^bk+1bk−1⊗qbkc0−1⊗qbk+1c0)vec2 ({\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}
根據上面的代價函數構造Ax = B即
JbwγTJbwγδbw=2JbwγT(r^bk+1bk−1⊗qbkc0−1⊗qbk+1c0)vec{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}
然後採用LDLT分解求得δbw\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]);
    }
}

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

該函數需要優化的變量有速度、重力加速度和尺度（論文式(16)）：
XI3(n+1)+3+1=[vb0b0,vb1b1,...,vbnbn,gc0,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]
其中，vbkbkv^{b_k}_{b_k}是第k幀圖像在其IMU座標系下的速度，gc0g^{c_0}是在第0幀相機座標系下的重力向量。
在IMU預積分中我們已經推導過論文式(5)：

Rwbkpbk+1w=Rwbk(pbkw+vbkwΔtk−12gwΔtk2)+αbk+1bkR^{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}}

Rwbkvbk+1w=Rwbk(vbkw−gwΔtk)+βbk+1bkR^{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}}

將c0c_0替代ww可以寫成：

αbk+1bk=Rc0bk(spbk+1c0−spbkc0+12gc0Δtk2−Rbkc0vbkbkΔtk)\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(Rbk+1c0vbk+1bk+1+gc0Δtk−Rbkc0vbkbk)\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})

將論文式(14)帶入αbk+1bk\alpha^{b_k}_{b_{k+1}}有：

δαbk+1bk=αbk+1bk−Rc0bk(spbk+1c0−spbkc0+12gc0Δtk2−Rbkc0vbkbkΔtk)\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−Rc0bk(spck+1c0−Rbk+1c0pcb−(spckc0−Rbkc0pcb)+12gc0Δtk2−Rbkc0vbkbkΔtk)=\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−Rc0bks(pck+1c0−pckc0)+Rc0bkRbk+1c0pcb−pcb+vbkbkΔtk−12Rc0bkgc0Δtk2=03×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}
這裏vbkbk,s,gc0v^{b_k}_{b_k},s,g^{c_0}是待優化變量，將其轉換成Hx=bHx=b的形式：

Rc0bk(pck+1c0−pckc0)s−vbkbkΔtk+12Rc0bkΔtk2gc0=αbk+1bk+Rc0bkRbk+1c0pcb−pcbR^{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
寫成矩陣形式：
[−IΔtk012Rc0bkΔtk2Rc0bk(pck+1c0−pckc0)][vbkbkvbk+1bk+1gc0s]=αbk+1bk+Rc0bkRbk+1c0pcb−pcb\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

對於βbk+1bk\beta^{b_k}_{b_{k+1}}有：

δβbk+1bk=βbk+1bk−Rc0bk(Rbk+1c0vbk+1bk+1+gc0Δtk−Rbkc0vbkbk)=03×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}
Rc0bkRbk+1c0vbk+1bk+1+Rc0bkΔtkgc0−Rc0bkRbkc0vbkbk=βbk+1bkR^{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}}

寫成矩陣形式：
[−IRc0bkRbk+1c0Rc0bkΔtk0][vbkbkvbk+1bk+1gc0s]=βbk+1bk\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}}

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

[−IΔtk012Rc0bkΔtk2Rc0bk(pck+1c0−pckc0)−IRc0bkRbk+1c0Rc0bkΔtk0][vbkbkvbk+1bk+1gc0s]=[αbk+1bk+Rc0bkRbk+1c0pcb−pcbβbk+1bk]\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]
即H6×10XI10×1=b6×1H^{6\times10}\mathcal X^{10\times1}_I=b^{6\times1}，使用LDLT分解求解：
HTHXI10×1=HTbH^TH\mathcal X^{10\times1}_I=H^Tb

對應代碼實現在以下函數中，因爲內容和上述理論一致就不放代碼了。裏面的MatrixXd tmp_A(6, 10)就是H，VectorXd tmp_b(6,1)就是b

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

最後求得尺度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^ˉ+w1b1+w2b2=∣∣g∣∣g^ˉ+b⃗3×2w2×1\hat g=||g||\bar{\hat g}+w_1b_1+w_2b_2=||g||\bar{\hat g}+\vec b^{3\times2}w^{2\times1}

其中w2×1=[w1,w2]T,b⃗3×2=[b1,b2]w^{2\times1}=[w_1,w_2]^T,\vec b^{3\times2}=[b_1,b_2]
g^ˉ\bar{\hat g}爲上一步求得的重力方向，,b1、b2b_1、b_2在g^\hat g的正切平面上且正交。b1b_1與b2b_2的設置爲：
在這裏插入圖片描述
將其帶入論文式(18)(19)中，有：

[−IΔtk012Rc0bkΔtk2b⃗Rc0bk(pck+1c0−pckc0)−IRc0bkRbk+1c0Rc0bkΔtkb⃗0][vbkbkvbk+1bk+1ws]=[δαbk+1bk+Rc0bkRbk+1c0pcb−pcb−12Rc0bkΔtk2∣∣g∣∣g^ˉβbk+1bk−Rc0bkΔtk∣∣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]
即H6×9XI9×1=b6×1H^{6\times9}\mathcal X^{9\times 1}_I=b^{6\times1}。使用LDLT分解求解：
HTHXI9×1=HTbH^TH\mathcal X^{9\times1}_I=H^Tb

在代碼實現中，以下函數用於重力細化，其流程基本對應上文推導，方程迭代求解4次：

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

函數中的MatrixXd lxly(3, 2)= TangentBasis(g0); 即對應b⃗3×2=[b1,b2]\vec b^{3\times2}=[b_1,b_2]，
VectorXd dg = x.segment<2>(n_state - 3); 即對應w2×1=[w1,w2]Tw^{2\times1}=[w_1,w_2]^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、根據對極幾何求解這兩幀的本質矩陣

        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的外參旋轉矩陣，根據等式：
Rck+1bk=Rbk+1bk⋅Rcb=Rcb⋅Rck+1ckR_{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}
其中Rbk+1bkR_{b_{k+1}}^{b_k}爲IMU座標系之間的旋轉矩陣，Rck+1ckR_{c_{k+1}}^{c_k}爲相機座標系之間的旋轉矩陣，RcbR_c^b爲從相機到IMU的旋轉矩陣。
用四元素重寫：
qbk+1bk⊗qcb=qcb⊗qck+1ckq_{b_{k+1}}^{b_k} \otimes q_c^b = q_c^b \otimes q_{c_{k+1}}^{c_k}
[Q1(qbk+1bk)−Q2(qck+1ck)]⋅qcb=Qcb⋅qcb=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
將多個幀之間的等式關係一起構建超定方程Ax=0Ax=0。對A進行svd分解，其中最小奇異值對應的右奇異向量便爲結果x，即旋轉四元數qcbq_c^b。

在代碼中，L爲相機旋轉四元數的左乘矩陣，R爲IMU旋轉四元數的右乘矩陣，因此其實構建的是：
qbc⊗qbk+1bk=qck+1ck⊗qbcq_b^c \otimes q_{b_{k+1}}^{b_k} =q_{c_{k+1}}^{c_k} \otimes q_b^c
所求的x是qbcq_b^c，在最後需要轉換成旋轉矩陣並求逆。

    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理論推導1

Reference VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator. Quaternion Kinematics for E

无人的回忆 2020-07-03 01:21:38

VINS-Mono理論推導2

Reference VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator. Quaternion Kinematics for E

无人的回忆 2020-07-07 01:02:40

VINS frontend - change feature's parent

VINS frontend - change feature's parentIntroductionVINS feature managerSwitch parent Introduction We want to change

hhhliuye 2020-07-05 14:33:51

《spring設計思想》18-bean知識總結

現在spring中的bean來源主要有兩個: 一個是掃描資源文件xml/properties等加載BeanDefinition 二是掃描Annotation標註的Class文件加載BeanDefinition 不管是哪一種，我們都知道讀取

David_lou 2020-07-07 21:30:59

VC++中構造函數初始化的方法以及主要區別

我的問題是關於初始化C++類成員的。我見過許多這樣的代碼（包括在你的欄目中也見到過）： CSomeClass::CSomeClass() {       x=0;       y=1;  } 而在別的什麼地方則寫成下面的樣子：

Antares_SANTALEN 2020-07-07 13:28:01

java中類的初始化探討

對於JAVA中類的初始化是一個很基礎的問題，其中的一些問題也是易被學習者所忽略。當在編寫代碼的時候碰到時，常被這些問題引發的錯誤，感覺莫名其妙。而且現在許多大公司的面試題，對於這方面的考查也是屢試不爽。不管基於什麼原因，我認爲，對於jav

春哥队长 2020-07-05 21:37:49

Springboot2.0--WebMvcConfigurer詳解

WebMvcConfigurer是spring內部配置的一種方式，使用JavaBean的方式代替傳統的xml配置；也可以自定義擴展配置類，實現方式是繼承WebMvcConfigurer接口；WebMvcConfigurer其實就

随风yy 2020-07-05 12:07:11

jquery跟js初始化加載的多種方法及區別介紹

jquery是等待頁面加載完數據，以及頁面部分元素；js是頁面全部加載完成才執行初始化加載，具體示例祥看本文 jquery和js初始化加載頁面的區別： jquery：等待頁面加載完數據，以及頁面部分元素（不包括圖片、視頻），

泡沫9水灵灵 2020-07-05 00:46:40

struts2源碼分析(二)(初始化)

2、Dispatcher dispatcher = init.initDispatcher(config);  這行代碼是struts2初始化的核心部分(該方法是InitOperations中的方法)方法的具體內容如下：  1:Dispa

sjcawn 2020-07-04 15:29:50

java9：一維數組的聲明，創建，初始化（single-dimensional arrays）

1 聲明與定義： 在說數組之前，還是得先說下聲明與定義：  變量的定義（definition）:用於爲變量分配存儲空間，還可以爲變量指定初始值。在一個程序中，變量有且僅有一個定義；  變量的聲明（declaration）：用於向程序表明變

dymom 2020-07-03 19:41:37

VIN0.Readme

VIN0.Readme0.introduction1.Reference2.Contents3.pipeline 0.introduction 打算記錄一下看代碼的流程。 1.Reference .　　後續博客主要參考以下資料，後

古路 2020-07-05 13:44:48

VIN1.視覺前端

VIN1.視覺前端0.引言1.流程1.1.readImage()函數2.主要視覺函數2.1.包含的視覺算法2.2.圖像顯示3.數據結構4.接入後端的數據結構4.1.IMU4.2.Image5.疑問 0.引言 搬運工+自己閱讀代碼理

古路 2020-07-05 13:44:48

VIN2.IMU預積分

VIN2.IMU預積分0.引言1.IMU測量模型2.當前時刻 PVQ 的連續形式3.當前時刻 PVQ 的中值法離散形式4.兩幀之間 PVQ 增量的連續形式5.兩幀之間 PVQ 增量的離散形式6.連續形式下 PVQ 增量的誤差、協方

古路 2020-07-05 13:44:48

VIN3.系統初始化

VIN3.系統初始化引言1.初始化流程2.CalibrationExRotation3.檢測 IMU 可觀性4.relativePose5.GlobalSFM.construct6.visualInitialAlign6.1.陀螺

古路 2020-07-05 13:44:48

攝像頭標定原理思考

數學原理： 標定中棋盤格角點像素座標(u, v)和世界座標(XW, YW, ZW)是已知的，根據方程的參數值用最小二乘法得出其他未知量的值。 式中內參矩陣參數： fx:在像素座標系x軸方向上一相機焦距f佔有的像素數，fy同理 內參矩陣第

tellw0.0 2020-07-01 08:28:45