VINS_by

在VINS初始化函数initialStructure()主要包含了两大步:第一步,就是上一节的纯视觉的SFM重建,得到滑窗中所有关键帧的位姿以及3D点坐标,并以此求得了包括非关键帧的位姿,并且这些位姿都是以 参考帧 l 为世界坐标得出的。第二步,就是本节的对齐,将所有求解出来的状态量对齐到第0帧IMU坐标系,也就是指视觉惯性对齐

回顾第一步,【VINS_Code-视觉惯导联合初始化之SFM】为了保证算法的效率,这一步保持了帧的滑动窗口来限制计算复杂度。首先,是使用五点法恢复参考帧和当前帧之间的位姿;然后,对这两帧之间的所有特征进行三角化,基于这些三角特征,采用PnP来估计窗口中所有其他帧的姿态;最后,应用全局BA 算法,最小化所有特征观测的重投影误差。

由于我们还没有关于世界坐标系的任何知识,我们将第一个相机坐标系 $(·)^{c_0}$ 设置为 SfM 的参考坐标系(可以理解为代码中的参考帧 l)。所有帧的位姿和特征位置 $(\overline {\mathbf{p} }_{c_k}^{c_0},{\mathbf{q} }_{c_k}^{c_0})$ 表示相对于 $(·)^{c_0}$ 坐标。假设摄像机和IMU之间有一个粗略测量的外部参数 $({\mathbf{p} }_c^b,{\mathbf{q} }_c^b)$【VINS_Code-旋转外参初始化】,我们可以将姿态从相机坐标系转换到物体 (IMU) 坐标系。

$$ \begin{align*} {\mathbf{q} }_{b_k}^{c_0}&={\mathbf{q} }_{c_k}^{c_0}\otimes({\mathbf{q} }_c^b)^{-1}\\ s\overline {\mathbf{p} }_{b_k}^{c_0}&=s\overline {\mathbf{p} }_{c_k}^{c_0}-{\mathbf{R} }_{b_k}^{c_0}{\mathbf{p} }_c^b \end{align*}\tag{1} $$

其中 $s$ 将视觉结构与公制比例对齐的缩放参数。解决这个缩放参数是实现初始化成功的关键。

推导:假设相机到IMU的外参为 $({\mathbf{p} }_c^b,{\mathbf{R} }_c^b)$ ,得到姿态从相机坐标系转换到IMU坐标系下的关系:

$$ \begin{align*} \mathbf{T}^{c_0}_{c_k}&=\mathbf{T}^{c_0}_{b_k}\mathbf{T}^{b}_{c}\\ \begin{bmatrix} \mathbf{R}^{c_0}_{c_k}&s\overline{\mathbf{p} }^{c_0}_{c_k}\\0&1 \end{bmatrix}&= \begin{bmatrix} \mathbf{R}^{c_0}_{b_k}&s\overline{\mathbf{p} }^{c_0}_{b_k}\\0&1 \end{bmatrix} \begin{bmatrix} \mathbf{R}^{b}_{c}&s\mathbf{p}^{b}_{c}\\0&1 \end{bmatrix}\\ &= \begin{bmatrix} \mathbf{R}^{c_0}_{b_k}\mathbf{R}^{b}_{c}& \mathbf{R}^{c_0}_{b_k}s\mathbf{p}^{b}_{c}+s\overline{\mathbf{p} }^{c_0}_{b_k} \\0&1 \end{bmatrix} \end{align*}\tag{1-1} $$

所以:

$$ \begin{align*} \mathbf{R}^{c_0}_{c_k}&=\mathbf{R}^{c_0}_{b_k}\mathbf{R}^{b}_{c}\\ \Rightarrow\mathbf{R}^{c_0}_{b_k}&=\mathbf{R}^{c_0}_{c_k}(\mathbf{R}^{b}_{c})^{-1} \\ s\overline{\mathbf{p} }^{c_0}_{c_k}&=\mathbf{R}^{c_0}_{b_k}s\mathbf{p}^{b}_{c}+s\overline{\mathbf{p} }^{c_0}_{b_k}\\ \Rightarrow s\overline{\mathbf{p} }^{c_0}_{b_k}&=s\overline{\mathbf{p} }^{c_0}_{c_k}-\mathbf{R}^{c_0}_{b_k}s\mathbf{p}^{b}_{c} \end{align*}\tag{1-2} $$

证毕!

至此,我们重新回忆一下,VINS所有的待优化变量有哪些:

状态向量共包括滑动窗口内的 $n+1$ 个所有 IMU 状态$\mathbf{x}_k,k\in[0,n]$(包括位置、朝向、速度、加速度计 $bias$ 和陀螺仪 $bias$)、 Camera 到 IMU 的外参 $\mathbf{x}^b_c$、 $m+1$ 个 3D 点的逆深度$\lambda_l,l\in[0,m]$:

$$ \begin{align*} \mathcal{X}&=[\mathbf{x}_0,\mathbf{x}_1,···,\mathbf{x}_n,\mathbf{x}^b_c,\lambda_0,\lambda_1,···,\lambda_m]\\ \mathbf{x}_k&=[\mathbf{p}^w_{b_k},\mathbf{v}^w_{b_k},\mathbf{q}^w_{b_k},\mathbf{b}_a,\mathbf{b}_g],k\in[0,n]\\ \mathbf{x}^b_c&=[\mathbf{p}^b_c,\mathbf{q}^b_c] \end{align*}\tag{2} $$

$n$ 是关键帧的总数,$m$ 是滑动窗口中的特征总数。 $λ_l$ 是第 $l$ 个特征与其第一次观察的逆深度。

本节将主要对IMU 状态$\mathbf{x}_k,k\in[0,n]$(包括位置、朝向、速度、加速度计 $bias$ 和陀螺仪 $bias$)进行初始化;

计算陀螺仪偏置,尺度,重力加速度和速度

视觉惯性对齐的示意图如图 1 所示。基本思想是将按比例放大的视觉结构与 IMU 预积分相匹配。

图 1. 估计器初始化的视觉惯性对齐过程示意图。其基本思想是用IMU预积分匹配up-to-scale的视觉结构。

image-20220711092649781

// 计算陀螺仪偏置,尺度,重力加速度和速度
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;
}

(1)陀螺仪偏置标定

考虑窗口中连续两帧 $b_k$ 和 $b_{k+1}$,我们从视觉 SfM 中得到旋转 ${\mathbf{q} }_{b_k}^{c_0}$ 和 ${\mathbf{q} }_{b_{k+1} }^{c_0}$ ,从IMU预积分得到的相对约束 $\hat{\gamma}^{b_k}_{b_{k+1} }$。我们对陀螺仪偏置求IMU预积分项的线性化,并最小化以下代价函数:

$$ \underset{\delta b_w}{\min}\sum_{k\in\mathcal{B} }\parallel { {\mathbf{q} }_{b_{k+1} }^{c_0} }^{-1}\otimes{\mathbf{q} }_{b_k}^{c_0}\otimes \gamma^{b_k}_{b_{k+1} } \parallel^2\\ \gamma^{b_k}_{b_{k+1} }\approx\hat{\gamma}^{b_k}_{b_{k+1} }\otimes\begin{bmatrix}1\\\frac{1}{2}\mathbf{J}^{\gamma}_{b_w}\delta b_{w} \end{bmatrix}\tag{3} $$

其中 $\mathcal{B}$ 是索引窗口中的所有帧。利用第四部分推导出的偏置雅可比矩阵,给出了 $\hat{\gamma}^{b_k}_{b_{k+1} }$ 对陀螺仪偏置的一阶近似。这样,我们得到了陀螺仪偏置 $b_w$ 的初始校准。然后我们用新的陀螺仪偏置重新传递所有的IMU预积分量 $ \hat{\alpha}^{b_k}_{b_{k+1} },\hat{\beta}^{b_k}_{b_{k+1} },\hat{\gamma}^{b_k}_{b_{k+1} }$。

由代价函数可知:

$$ { {\mathbf{q} }_{b_{k+1} }^{c_0} }^{-1}\otimes{\mathbf{q} }_{b_k}^{c_0}\otimes \gamma^{b_k}_{b_{k+1} }=\begin{bmatrix}1\\0\end{bmatrix}\tag{4} $$

所以:

$$ \gamma^{b_k}_{b_{k+1} }= { {\mathbf{q} }_{b_k }^{c_0} }^{-1}\otimes{\mathbf{q} }_{b_{k+1} }^{c_0}\otimes\begin{bmatrix}1\\0\end{bmatrix}\tag{5} $$

代入 $\delta b_w$ :

$$ \gamma^{b_k}_{b_{k+1} }\approx\hat{\gamma}^{b_k}_{b_{k+1} }\otimes\begin{bmatrix}1\\\frac{1}{2}\mathbf{J}^{\gamma}_{b_w}\delta b_{w} \end{bmatrix}= { {\mathbf{q} }_{b_k }^{c_0} }^{-1}\otimes{\mathbf{q} }_{b_{k+1} }^{c_0}\otimes\begin{bmatrix}1\\0\end{bmatrix}\tag{6} $$

只考虑虚部,有:

$$ \begin{align*} \mathbf{J}^{\gamma}_{b_w}\delta b_{w}&=2[( {\hat{\gamma}^{b_k}_{b_{k+1} } }^{-1}\otimes { {\mathbf{q} }_{b_k }^{c_0} }^{-1}\otimes{\mathbf{q} }_{b_{k+1} }^{c_0})]_{vec} \\ \Rightarrow {\mathbf{J}^{\gamma}_{b_w} }^{T}\mathbf{J}^{\gamma}_{b_w}\delta b_{w}&=2{\mathbf{J}^{\gamma}_{b_w} }^{T} [({\hat{\gamma}^{b_k}_{b_{k+1} } }^{-1}\otimes { {\mathbf{q} }_{b_k }^{c_0} }^{-1}\otimes{\mathbf{q} }_{b_{k+1} }^{c_0})]_{vec} \end{align*}\tag{7} $$

用LDLT分解求得 $\delta b_w$ .

/**
 * @brief 对陀螺仪的偏置进行标定
 * 
 * @param all_image_frame 
 * @param Bgs 
 */
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;
    // 求解连续两帧之间的bais的标定
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
    {
        frame_j = next(frame_i);
        
        // 构造 AX=b
        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;

    }
    delta_bg = A.ldlt().solve(b); // 进行LDLT分解
    ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());
    // 滑窗中的零偏,设置为求解出的零偏
    for (int i = 0; i <= WINDOW_SIZE; i++)
        Bgs[i] += delta_bg;
    //对all_image_frame中预积分量根据当前零偏重新积分。
    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]);
    }
}

在之前讨论过过,在论文中提到:当 $bais$ 的估计发生变化时,如果变化很小,我们通过它们对 $bais$ 的一阶近似来调整 $\alpha^{b_k}_{b_{k+1} },\beta^{b_k}_{b_{k+1} },\gamma^{b_k}_{b_{k+1} }$ ,否则我们进行重新传播。这种策略为基于优化的算法节省了大量的计算资源,因为我们不需要重复传播 IMU 测量。一阶近似如下:

$$ \begin{align*} \alpha^{b_k}_{b_{k+1} }&\approx \hat{\alpha}^{b_k}_{b_{k+1} }+\mathbf{J}^{\alpha}_{b_a}\delta b_{a_k}+\mathbf{J}^{\alpha}_{b_w}\delta b_{w_k} \\ \beta^{b_k}_{b_{k+1} }&\approx \hat{\beta}^{b_k}_{b_{k+1} }+\mathbf{J}^{\beta}_{b_a}\delta b_{a_k}+\mathbf{J}^{\beta}_{b_w}\delta b_{w_k} \\ \gamma^{b_k}_{b_{k+1} }&\approx \hat{\gamma}^{b_k}_{b_{k+1} }\otimes \begin{bmatrix}1\\\frac{1}{2}\mathbf{J}^{\gamma}_{b_w}\delta b_{w_k} \end{bmatrix} \end{align*} $$

其中 $\mathbf{J}^{\alpha}_{b_a}$ 和 是 $\mathbf{J}^{\alpha}_{b_{k+1} }$ 中的子块矩阵,其位置对应于 $\frac{\delta\alpha^{b_k}_{b_{k+1} } }{\delta b_{a_k} }$ 。 $\mathbf{J}^{\alpha}_{b_w}、\mathbf{J}^{\beta}_{b_a}、\mathbf{J}^{\beta}_{b_w}、\mathbf{J}^{\gamma}_{b_w}$ 也使用相同的含义。

但是,在此处,仍然进行了重新传播,这是因为,在初始化当中,我们的bais初始值是0,bais的估计误差并不是非常小,因此为了计算的准确性,在初始化中,我们需要对bais进行重新传播,进行预积分操作,但是在后端优化中则直接使用计算的雅可比矩阵对预积分量进行调整。

(2)速度、重力向量和尺度初始化

在陀螺仪偏置初始化后,我们继续初始化导航的其他基本状态,即速度、重力向量和尺度:

$$ \mathcal{X}_I=[\mathbf{v}^{b_0}_{b_0},\mathbf{v}^{b_1}_{b_1},···,\mathbf{v}^{b_n}_{b_n},g^{c_0},s]\tag{8} $$

其中 $\mathbf{v}^{b_k}_{b_k}$ 是拍摄第 $k$ 张图像时本体坐标系中的速度,$g^{c_0}$ 是 $c_0$ 帧中的重力矢量,$s$ 将单目 SfM 缩放为公制单位。

由预积分公式:

$$ \begin{align*} &\left\{\begin{array}{L} \mathbf{R}^{b_k}_w\mathbf{p}^w_{b_{k+1} }=\mathbf{R}^{b_k}_w(\mathbf{p}^w_{b_k}+\mathbf{v}^w_{b_k}\Delta t_k- \frac{1}{2}g^w\Delta t_k^2)+\alpha^{b_k}_{b_{k+1} }\\ \mathbf{R}^{b_k}_w\mathbf{v}^w_{b_{k+1} }= \mathbf{R}^{b_k}_w(\mathbf{v}^w_{b_k}-g^w\Delta t_k)+\beta^{b_k}_{b_{k+1} } \end{array}\right. \\ \Rightarrow &\left\{\begin{array}{L} \alpha^{b_k}_{b_{k+1} }=\mathbf{R}^{b_k}_w(\mathbf{p}^w_{b_{k+1} }-\mathbf{p}^w_{b_k}+\frac{1}{2}g^w\Delta t_k^2-\mathbf{v}^w_{b_k}\Delta t_k)\\ \beta^{b_k}_{b_{k+1} }=\mathbf{R}^{b_k}_w(\mathbf{v}^w_{b_{k+1} }+g^w\Delta t_k-\mathbf{v}^w_{b_k}) \end{array}\right. \end{align*}\tag{9} $$

由于目前还没有对齐到世界坐标系下去,因此,需要同意转换到参考帧的坐标系下,即 $(\cdot)^{c_0}$:

$$ \left\{\begin{array}{L} \alpha^{b_k}_{b_{k+1} }=\mathbf{R}^{b_k}_{c_0}(\mathbf{p}^{c_0}_{b_{k+1} }-\mathbf{p}^{c_0}_{b_k}+\frac{1}{2}g^{c_0}\Delta t_k^2-\mathbf{v}^{c_0}_{b_k}\Delta t_k)\\ \beta^{b_k}_{b_{k+1} }=\mathbf{R}^{b_k}_{c_0}(\mathbf{v}^{c_0}_{b_{k+1} }+g^{c_0}\Delta t_k-\mathbf{v}^{c_0}_{b_k}) \end{array}\right.\tag{10} $$

由于平移是具有尺度的,因此平移前面需要加上后面估计的尺度信息 $s$,然后根据式 $(1)$ 可以得到:

$$ \left\{\begin{array}{L} \alpha^{b_k}_{b_{k+1} }=\mathbf{R}^{b_k}_{c_0}([s\mathbf{p}^{c_0}_{c_{k+1} }-\mathbf{R}^{c_0}_{b_{k+1} }\mathbf{p}^b_c]-[s\mathbf{p}^{c_0}_{c_{k} }-\mathbf{R}^{c_0}_{b_{k} }\mathbf{p}^b_c]+\frac{1}{2}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}_{b_{k+1} }=\mathbf{R}^{b_k}_{c_0}(\mathbf{R}_{b_{k+1} }^{c_0}\mathbf{v}^{b_{k+1} }_{b_{k+1} }+g^{c_0}\Delta t_k-\mathbf{R}_{b_k}^{c_0}\mathbf{v}^{b_k}_{b_k}) \end{array}\right.\tag{11} $$

把已知量移到等式右侧:

$$ \left\{\begin{array}{L} \mathbf{R}^{b_k}_{c_0}(s(\overline{\mathbf{p} }_{b_{k+1} }^{c_0}-\overline{\mathbf{p} }_{b_{k} }^{c_0})+\frac{1}{2}g^{c_0}\Delta t^2_k-\mathbf{R}_{b_k}^{c_0}\mathbf{v}^{b_k}_{b_k}\Delta t_k)=\alpha^{b_k}_{b_{k+1} }+\mathbf{R}^{b_k}_{c_0}{\mathbf{R} }_{b_{k+1} }^{c_0}{\mathbf{p} }_c^b-{\mathbf{p} }_c^b\\ \beta^{b_k}_{b_{k+1} }=\mathbf{R}^{b_k}_{c_0}(\mathbf{R}_{b_{k+1} }^{c_0}\mathbf{v}^{b_{k+1} }_{b_{k+1} }+g^{c_0}\Delta t_k-\mathbf{R}_{b_k}^{c_0}\mathbf{v}^{b_k}_{b_k}) \end{array}\right.\tag{12} $$

  • 对于 $\delta\alpha^{b_k}_{b_{k+1} }$,等式两边相减

$$ \begin{align*} \delta\alpha^{b_k}_{b_{k+1} }= \alpha^{b_k}_{b_{k+1} }+\mathbf{R}^{b_k}_{c_0}{\mathbf{R} }_{b_{k+1} }^{c_0}{\mathbf{p} }_c^b-{\mathbf{p} }_c^b -\mathbf{R}^{b_k}_{c_0}s(\overline{\mathbf{p} }_{b_{k+1} }^{c_0} -\overline{\mathbf{p} }_{b_{k} }^{c_0}) -\frac{1}{2}\mathbf{R}^{b_k}_{c_0}g^{c_0}\Delta t^2_k +\mathbf{v}^{b_k}_{b_k}\Delta t_k\\ \Rightarrow -\mathbf{v}^{b_k}_{b_k}\Delta t +\frac{1}{2}\mathbf{R}^{b_k}_{c_0}\Delta t^2_kg^{c_0}+ \mathbf{R}^{b_k}_{c_0} (\overline {\mathbf{p} }_{c_{k+1} }^{c_0}-\overline {\mathbf{p} }_{c_k}^{c_0})s+\delta\alpha^{b_k}_{b_{k+1} } =\alpha^{b_k}_{b_{k+1} }+\mathbf{R}^{b_k}_{c_0}{\mathbf{R} }_{b_{k+1} }^{c_0}{\mathbf{p} }_c^b-{\mathbf{p} }_c^b \end{align*}\tag{13} $$

写成矩阵形式:

$$ \begin{align*} \begin{bmatrix} -\mathbf{I}\Delta t_k&0&\frac{1}{2}\mathbf{R}^{b_k}_{c_0}\Delta t_k^2&\mathbf{R}^{b_k}_{c_0}(\overline {\mathbf{p} }_{c_{k+1} }^{c_0}-\overline {\mathbf{p} }_{c_k}^{c_0}) \end{bmatrix} \begin{bmatrix} \mathbf{v}^{b_k}_{b_k}\\\mathbf{v}^{b_{k+1} }_{b_{k+1} }\\ g^{c_0}\\s \end{bmatrix} =\hat{\alpha}^{b_k}_{b_{k+1} }+\mathbf{R}^{b_k}_{c_0}{\mathbf{R} }_{b_{k+1} }^{c_0}{\mathbf{p} }_c^b-{\mathbf{p} }_c^b \end{align*}\tag{14} $$

  • 对于 $\delta\beta^{b_k}_{b_{k+1} }$,等式两边相减

$$ \begin{align*} \delta\beta^{b_k}_{b_{k+1} }&=\beta^{b_k}_{b_{k+1} }-\mathbf{R}^{b_k}_{c_0}(\mathbf{R}_{b_{k+1} }^{c_0}\mathbf{v}^{b_{k+1} }_{b_{k+1} }+g^{c_0}\Delta t_k-\mathbf{R}_{b_k}^{c_0}\mathbf{v}^{b_k}_{b_k})\\ &\Rightarrow \mathbf{R}^{b_k}_{c_0} (\mathbf{R}_{b_{k+1} }^{c_0}\mathbf{v}^{b_{k+1} }_{b_{k+1} }+g^{c_0}\Delta t_k-\mathbf{R}_{b_k}^{c_0}\mathbf{v}^{b_k}_{b_k})+\delta\beta^{b_k}_{b_{k+1} }=\beta^{b_k}_{b_{k+1} }\\ &\Rightarrow -\mathbf{R}^{b_k}_{c_0} \mathbf{R}_{b_k}^{c_0}\mathbf{v}^{b_k}_{b_k} +\mathbf{R}^{b_k}_{c_0} \mathbf{R}_{b_{k+1} }^{c_0}\mathbf{v}^{b_{k+1} }_{b_{k+1} } +\mathbf{R}^{b_k}_{c_0}\Delta t_kg^{c_0}+\delta\beta^{b_k}_{b_{k+1} }=\beta^{b_k}_{b_{k+1} }\\ &\Rightarrow -\mathbf{v}^{b_k}_{b_k} +\mathbf{R}^{b_k}_{c_0} \mathbf{R}_{b_{k+1} }^{c_0}\mathbf{v}^{b_{k+1} }_{b_{k+1} } +\mathbf{R}^{b_k}_{c_0}\Delta t_kg^{c_0}+\delta\beta^{b_k}_{b_{k+1} }=\beta^{b_k}_{b_{k+1} }\\ \end{align*}\tag{15} $$

写成矩阵形式:

$$ \begin{align*} \begin{bmatrix} -\mathbf{I}&\mathbf{R}^{b_k}_{c_0}\mathbf{R}^{c_0}_{b_{k+1} }&\mathbf{R}^{b_k}_{c_0}\Delta t_k&0 \end{bmatrix} \begin{bmatrix} \mathbf{v}^{b_k}_{b_k}\\\mathbf{v}^{b_{k+1} }_{b_{k+1} }\\ g^{c_0}\\s \end{bmatrix} =\hat{\beta}^{b_k}_{b_{k+1} } \end{align*}\tag{16} $$

由此可得:

$$ \begin{bmatrix} -\mathbf{I}\Delta t_k&0&\frac{1}{2}\mathbf{R}^{b_k}_{c_0}\Delta t_k^2&\mathbf{R}^{b_k}_{c_0}(\overline {\mathbf{p} }_{c_{k+1} }^{c_0}-\overline {\mathbf{p} }_{c_k}^{c_0})\\ -\mathbf{I}&\mathbf{R}^{b_k}_{c_0}\mathbf{R}^{c_0}_{b_{k+1} }&\mathbf{R}^{b_k}_{c_0}\Delta t_k&0 \end{bmatrix} \begin{bmatrix} \mathbf{v}^{b_k}_{b_k}\\\mathbf{v}^{b_{k+1} }_{b_{k+1} }\\ g^{c_0}\\s \end{bmatrix} = \begin{bmatrix} \hat{\alpha}^{b_k}_{b_{k+1} }-\mathbf{p}^b_c+\mathbf{R}^{b_k}_{c_0}\mathbf{R}^{c_0}_{b_{k+1} }\mathbf{p}^b_c\\ \hat{\beta}^{b_k}_{b_{k+1} } \end{bmatrix}\tag{17} $$

可以简写为:

$$ {\mathbf{H}^{b_k}_{b_{k+1} } }^{6\times10}{\mathcal{X}_I}^{10\times1}={\hat{\mathbf{z} }^{b_k}_{b_{k+1} } }^{6\times1}\tag{18} $$

其中:

$$ \begin{align*} \mathbf{H}^{b_k}_{b_{k+1} }&= \begin{bmatrix} -\mathbf{I}\Delta t_k&0&\frac{1}{2}\mathbf{R}^{b_k}_{c_0}\Delta t_k^2&\mathbf{R}^{b_k}_{c_0}(\overline {\mathbf{p} }_{c_{k+1} }^{c_0}-\overline {\mathbf{p} }_{c_k}^{c_0})\\ -\mathbf{I}&\mathbf{R}^{b_k}_{c_0}\mathbf{R}^{c_0}_{b_{k+1} }&\mathbf{R}^{b_k}_{c_0}\Delta t_k&0 \end{bmatrix}\\ \hat{\mathbf{z} }^{b_k}_{b_{k+1} }&= \begin{bmatrix} \hat{\alpha}^{b_k}_{b_{k+1} }-\mathbf{p}^b_c+\mathbf{R}^{b_k}_{c_0}\mathbf{R}^{c_0}_{b_{k+1} }\mathbf{p}^b_c\\ \hat{\beta}^{b_k}_{b_{k+1} } \end{bmatrix} \end{align*} \tag{19} $$

可以看出,$\mathbf{R}^{b_k}_{c_0},\ \mathbf{R}^{c_0}_{b_{k+1} },\ \overline {\mathbf{p} }_{c_k}^{c_0}$ 和 $ \overline {\mathbf{p} }_{c_{k+1} }^{c_0}$ 是从up-to-scale 单目视觉SfM 中得到的。 $\Delta t_k$ 是两个连续帧之间的时间间隔。这是一个线性最小二乘问题,代价函数为:

$$ \underset{\mathcal{X}_I}{\min}\sum_{k\in\mathcal{B} }\parallel\hat{\mathbf{z} }^{b_k}_{b_{k+1} }-\mathbf{H}^{b_k}_{b_{k+1} }\mathcal{X}_I \parallel^2\tag{20} $$

由代价函数可知:

$$ \mathbf{H}^{b_k}_{b_{k+1} }\mathcal{X}_I=\hat{\mathbf{z} }^{b_k}_{b_{k+1} }\tag{21} $$

然后使用LDLT分解求解 $\mathcal{X}_I$:

$$ \begin{align*} {\mathbf{H}^{b_k}_{b_{k+1} } }^T\mathbf{H}^{b_k}_{b_{k+1} }\mathcal{X}_I={\mathbf{H}^{b_k}_{b_{k+1} } }^T\hat{\mathbf{z} }^{b_k}_{b_{k+1} }\\ \end{align*}\tag{22} $$

代码中,$\mathbf{H}^{b_k}_{b_{k+1} }$ 为 $A$, $\hat{\mathbf{z} }^{b_k}_{b_{k+1} }$ 为 $b$。

这样我们可以获得窗口中每一帧本体坐标系的速度、视觉参考帧 $(·)^{c_0}$ 中的重力矢量以及比例参数。

/**
 * @brief 估计尺度、重力以及速度
 * 
 * @param all_image_frame 
 * @param g 
 * @param x 
 * @return true 
 * @return false 
 */
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; //3(N+1)+1

    // A b 是最终的大矩阵,它是由小矩阵tmp_A tmp_b 累加得到的
    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;
    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); // H
        tmp_A.setZero();
        VectorXd tmp_b(6);  // Z
        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();

        // H^T * H * x = H^T*b
        MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A; // H^T * H
        VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b; // H^T * b
        
        // A = H^T * H 是一个 10*10 的矩阵 
        A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>(); // 左上角 6*6 的矩阵块与 v_k v_{k+1} 相关
        b.segment<6>(i * 3) += r_b.head<6>(); // 6*1 : v_k v_{k+1}

        A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>(); // 右下角 4*4 的矩阵与 g s 相关
        b.tail<4>() += r_b.tail<4>(); // 4*1 :  g s 

        A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();// 右上角 6*4
        A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();// 左下角 4*6
    }
    A = A * 1000.0;
    b = b * 1000.0;
    x = A.ldlt().solve(b);
    // 求得真实尺度需除以100 (取出最后一维)
    double s = x(n_state - 1) / 100.0;
    ROS_DEBUG("estimated scale: %f", s);
    // 求得重力(取出最后四维的前三维)
    g = x.segment<3>(n_state - 4);
    ROS_DEBUG_STREAM(" result g     " << g.norm() << " " << g.transpose());
    //如果重力加速度与参考值差(G=9.81)太大或者尺度为负则说明计算错误
    if(fabs(g.norm() - G.norm()) > 1.0 || s < 0)
    {
        return false;
    }
    // 重力细化
    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;
}

(3)重力细化

通过约束量值,可以对原线性初始化步骤得到的重力向量进行细化。在大多数情况下,重力向量的大小是已知的。这导致重力向量只剩2个自由度。因此,我们在其切线空间上用两个变量重新参数化重力。

我们的重力向量受到 $g(\overline{\hat{g} } + \delta g)$ 的扰动,$\delta g = w_1b_1 + w_2b_2$,其中 $g$ 是已知的重力大小,$\overline{\hat{g} }$ 是表示重力方向的单位向量。 $b_1$ 和 $b_2$ 是跨越切平面的两个正交基,如图所示。 $w_1$ 和 $w_2$ 分别是对 $b_1$ 和 $b_2$ 的二维扰动。我们可以任意找到任何旋转切线空间的 $b_1$ 和 $b_2$ 集合。

自由度重力扰动示意图。由于重力的大小是已知的,$g$ 位于半径 $g ≈ 9.81 m/s^2$ 的球体上。重力在当前估计附近受到扰动,因为 $g(\overline{\hat{g} } + \delta g)$ ,$\delta g = w_1b_1 + w_2b_2$, 其中 $b_1$ 和 $b_2$ 是跨越切线空间的两个正交基。

$b_1$ 和 $b_2$ 的求取利用如下算法:

对应代码:

// 半径为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(); // a.transpose() * tmp=cos\theta
    c = a.cross(b); // a 叉乘 b 
    MatrixXd bc(3, 2);
    bc.block<3, 1>(0, 0) = b;
    bc.block<3, 1>(0, 1) = c;
    return bc;
}

$$ \hat{g}=\parallel g\parallel\overline{\hat{g} }+w_1b_1 + w_2b_2= \parallel g\parallel\overline{\hat{g} }+\vec{b}^{3\times2}\vec{w}^{2\times1}\tag{23} $$

其中,$\vec{b}^{3\times2}=[b_1,b_2],\vec{w}^{2\times1}=[w_1,w_2]^T.$

然后,我们用 $g(\overline{\hat{g} } + \delta g)$ 将 $g$ 代入 $(9)$,并与其他状态变量一起求解 维 $δg$。这个过程迭代了几次,直到 $\hat{g}$ 收敛。

$$ \begin{bmatrix} -\mathbf{I}\Delta t_k&0&\frac{1}{2}\mathbf{R}^{b_k}_{c_0}\Delta t_k^2\ \vec{b}&\mathbf{R}^{b_k}_{c_0}(\overline {\mathbf{p} }_{c_{k+1} }^{c_0}-\overline {\mathbf{p} }_{c_k}^{c_0})\\ -\mathbf{I}&\mathbf{R}^{b_k}_{c_0}\mathbf{R}^{c_0}_{b_{k+1} }&\mathbf{R}^{b_k}_{c_0}\Delta t_k\ \vec{b}&0 \end{bmatrix} \begin{bmatrix} \mathbf{v}^{b_k}_{b_k}\\\mathbf{v}^{b_{k+1} }_{b_{k+1} }\\ \vec{w}\\s \end{bmatrix} = \begin{bmatrix} \hat{\alpha}^{b_k}_{b_{k+1} }-\mathbf{p}^b_c+\mathbf{R}^{b_k}_{c_0}\mathbf{R}^{c_0}_{b_{k+1} }\mathbf{p}^b_c -\frac{1}{2}\mathbf{R}^{b_k}_{c_0}\Delta t_k^2\parallel g\parallel\overline{\hat{g} }\\ \hat{\beta}^{b_k}_{b_{k+1} }-\mathbf{R}^{b_k}_{c_0}\Delta t_k\parallel g\parallel\overline{\hat{g} } \end{bmatrix}\tag{24} $$

可以简写为:

$$ {\mathbf{H}^{b_k}_{b_{k+1} } }^{6\times9}{\mathcal{X}_I}^{9\times1}={\hat{\mathbf{z} }^{b_k}_{b_{k+1} } }^{6\times1}\tag{25} $$

使用LDLT分解求解 $\mathcal{X}_I$:

$$ {\mathbf{H}^{b_k}_{b_{k+1} } }^T{\mathbf{H}^{b_k}_{b_{k+1} } }{\mathcal{X}_I}={\mathbf{H}^{b_k}_{b_{k+1} } }^T{\hat{\mathbf{z} }^{b_k}_{b_{k+1} } }\tag{26} $$

image-20220524152548316

/**
 * @brief 重力细化
 * 
 * @param all_image_frame 
 * @param g 
 * @param x 
 */
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;
    for(int k = 0; k < 4; k++)
    {
        MatrixXd lxly(3, 2);
        lxly = TangentBasis(g0);
        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();   // A
            VectorXd tmp_b(6); // b
            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>();
        }
            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;
}

视觉重建结果与IMU结果进行对齐

image-20220712110858925

该函数主要实现了陀螺仪的偏置校准 (加速度偏置没有处理),计算速度 V[0:n]、重力 g、尺度 s。同时更新了 Bgs 后,IMU 测量量需要 repropagate;得到尺度 s 和重力 g 的方向后,需更新所有图像帧在世界坐标系下的 Ps、Rs、Vs。

bool Estimator::visualInitialAlign()
{
    TicToc t_g;
    VectorXd x;
    // solve scale
    //O_ Step1、计算陀螺仪偏置,尺度,重力加速度和速度
    bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);
    if (!result)
    {
        ROS_DEBUG("solve g failed!");
        return false;
    }

    // change state
    //O_ Step2、将对齐后的位姿赋给滑窗中的值,并将其置为关键帧
    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;
    }

    //O_ Step3、重新计算所有特征点的深度
    VectorXd dep = f_manager.getDepthVector(); // 根据有效特征点,初始化这个动态向量
    for (int i = 0; i < dep.size(); i++) // 将所有特征点的深度置为-1
        dep[i] = -1;                     // 深度预设都是-1
    f_manager.clearDepth(dep);           // 特征点管理器把所有特征点逆深度也设为-1 

    // triangulat on cam pose , no tic
    //重新计算特征点的深度
    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]));

    //O_ Step4、陀螺仪的偏置 bgs 改变,重新计算预积分
    double s = (x.tail<1>())(0);
    for (int i = 0; i <= WINDOW_SIZE; i++)
    {
        pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
    }

    //O_ Step5、将 Ps、Vs、depth 尺度 s 缩放后转变为相对于第 0 帧IMU坐标系下
    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;
    // 把求解出来的KF速度赋给滑窗中
    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);
        }
    }
    // 把尺度模糊的3d点恢复到真实尺度下
    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;
    }

    // O_ Step6、通过将重力旋转到 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 * Rs[0].transpose();
    Matrix3d rot_diff = R0;

    // O_ Step7、所有变量从参考坐标系 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];
    }
    ROS_DEBUG_STREAM("g0     " << g.transpose());
    ROS_DEBUG_STREAM("my R0  " << Utility::R2ypr(Rs[0]).transpose());

    return true;
}
🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/82.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 07 月 12 日
赏杯咖啡