VINS_MONO系列:(四)紧耦合VIO实现

目录

1、写在前面

2、算法推导

2.1、IMU因子

2.2、视觉因子

2.3、边缘化因子

3、代码实现

4、反思与探讨

5、参考文献


相关链接

 VINS_MONO系列:(一)总体框架_Derrr...的博客-CSDN博客_vins框架

VINS_MONO系列:(二)IMU预积分详细推导_Derrr...的博客-CSDN博客

VINS_MONO系列:(三)VIO初始化_Derrr...的博客-CSDN博客

VINS_MONO系列:(四)紧耦合VIO实现_Derrr...的博客-CSDN博客

 VINS_MONO系列:(五)前端特征提取_Derrr...的博客-CSDN博客

1、写在前面

这篇文章主要介绍VINS的整个VIO的实现方法,包括理论的公式推导部分和代码部分,也就是对应的esimator部分的代码内容。理论部分主要介绍三种因子(IMU预积分因子、视觉因子和边缘化因子)的残差构造及雅克比推导,代码部分主要展示如何通过ceres构造这三种因子。

2、算法推导

VINS的VIO是基于紧耦合框架实现的,其本质就是一个基于滑动窗口的和图优化的VIO,整个紧耦合VIO的示意图如图1所示。

VINS_MONO系列:(四)紧耦合VIO实现_第1张图片

图1

整个包含的状态量有

VINS_MONO系列:(四)紧耦合VIO实现_第2张图片

其中\small \chi为状态向量,\small x_k为第K帧的IMU状态量,\small x^b_c为相机与IMU间的外参,\small \lambda为特征的逆深度值。

整个VIO的优化问题可以表示为

\small \min_{\chi }\left \{ \left \| r_p-H_p\chi \right \|^2 + \sum_{k\in B}^{}\left \|r_B(\hat{z}^{b_k}_{b_{k+1}},\chi ) \right \|^2_{P^{b_k}_{b_{k+1}}}+\sum_{(l,j)\in C}^{} \rho (\left \| r_C(\hat{z}^{C_j}_l,\chi) \right \|^2_{P^{C_j}_{l}})\right \}

式中第一项代表边缘化残差,第二项对应IMU残差,第三项对应视觉重投影残差,其中\small \left \| . \right \|表示马氏距离,\small {P^{b_k}_{b_{k+1}}},{P^{C_j}_{l}}分别为IMU因子和视觉因子的信息矩阵,下面分别介绍各个因子的残差构成及对应雅克比的推导,最后再对外参标定和同步时间标定进行介绍。

2.1、IMU因子

VINS中的IMU因子与传统[2]IMU预积分的形式不同,其具体的残差和雅克比推导可以参考我的另一篇文章,这里就不再进行赘述。

 VINS_MONO系列:(二)IMU预积分详细推导_Derrr...的博客-CSDN博客

2.2、视觉因子

视觉因子中的残差即为重投影误差,其本质为不同位姿对于同一个landmark的观察应该是一致的,即将空间中的landmark投影到各个能观察到该landmark的关键帧中,其投影的像素坐标及真实的观测值应该是重合的。但由于各种误差的存在,这个投影坐标及观察值往往不完全重合,而这中间的差值即为对应的重投影误差。视觉因子中优化的状态量为

\small \chi =[p^w_{b_i},p^w_{b_j}],[q^w_{b_i},q^w_{b_j}],[p^b_{c},q^b_{c}],[\lambda ]

原文视觉残差的推导会将残差投影到一个正切平面上再进行计算,但是在实际代码实现上还是使用了原本的重投影误差,为了和代码保持一致,这里对原本的重投影误差进行推导

对于空间中的一个特征点P,其第j帧的归一化平面坐标为\small P^{C_j}_l,第j帧的观测值为\small \hat{P}^{C_j}_l,则其对应的残差可以表示为

\small r_c=P^{C_j}_l-\hat{P}^{C_j}_l

其中

\small P^{C_j}_l=R^c_b\left \{R^{b_j}_w[R^w_{b_i}(R^b_c\frac{1}{\lambda }\hat{P}^{C_i}_l+p^b_c)+p^w_{b_i}-p^w_{b_j}] -p^b_c\right \}

其中\small \hat{P}^{C_i}_l为第i帧第l个特征归一化平面的观测值。

各项雅克比为

\small \frac{\partial r_c}{\partial p^w_{b_i}}=R^c_bR^{b_j}_w

\small \frac{\partial r_c}{\partial p^w_{b_j}}=-R^c_bR^{b_j}_w

\small \frac{\partial r_c}{\partial q^w_{b_i}}=\lim_{\delta \theta \rightarrow 0}\frac{R^c_b\left \{R^{b_j}_w[R^w_{b_i}Exp(\delta \theta )(\cdot )+p^w_{b_i}-p^w_{b_j}] -p^b_c\right \}-R^c_b\left \{R^{b_j}_w[R^w_{b_i}(\cdot )+p^w_{b_i}-p^w_{b_j}] -p^b_c\right \}}{\delta \theta}

\small \approx \lim_{\delta \theta \rightarrow 0}\frac{R^c_b\left \{R^{b_j}_w[R^w_{b_i}(I+\delta \theta )(\cdot )+p^w_{b_i}-p^w_{b_j}] -p^b_c\right \}-R^c_b\left \{R^{b_j}_w[R^w_{b_i}(\cdot )+p^w_{b_i}-p^w_{b_j}] -p^b_c\right \}}{\delta \theta}

\small =\lim_{\delta \theta \rightarrow 0}\frac{R^c_b\left \{R^{b_j}_w[R^w_{b_i}(\delta \theta )^{\wedge }(\cdot )] \right \}}{\delta \theta}=-R^c_bR^{b_j}_wR^w_{b_i}(\cdot )^{\wedge }

\small \frac{\partial r_c}{\partial q^w_{b_j}}=\lim_{\delta \theta \rightarrow 0}\frac{R^c_b\left \{[R^w_{b_j}Exp(\delta \theta )]^{-1}[R^w_{b_i}(\cdot )+p^w_{b_i}-p^w_{b_j}] -p^b_c\right \}-R^c_b\left \{R^{b_j}_w[R^w_{b_i}(\cdot )+p^w_{b_i}-p^w_{b_j}] -p^b_c\right \}}{\delta \theta}

\small \approx \lim_{\delta \theta \rightarrow 0}\frac{R^c_b\left \{[R^w_{b_j}(I+\delta \theta )]^{-1}[R^w_{b_i}(\cdot )+p^w_{b_i}-p^w_{b_j}] -p^b_c\right \}-R^c_b\left \{R^{b_j}_w[R^w_{b_i}(\cdot )+p^w_{b_i}-p^w_{b_j}] -p^b_c\right \}}{\delta \theta}

\small = \lim_{\delta \theta \rightarrow 0}\frac{R^c_b\left \{-\delta \theta ^{\wedge }[R^w_{b_j}]^{-1}[R^w_{b_i}(\cdot )+p^w_{b_i}-p^w_{b_j}] \right \}}{\delta \theta}=\lim_{\delta \theta \rightarrow 0}\frac{R^c_b\left \{[R^w_{b_j}]^{-1}[R^w_{b_i}(\cdot )+p^w_{b_i}-p^w_{b_j}]\right \}^{\wedge }\delta \theta }{\delta \theta}

\small =R^c_b\left \{[R^w_{b_j}]^{-1}[R^w_{b_i}(\cdot )+p^w_{b_i}-p^w_{b_j}]\right \}^{\wedge }

\small \frac{\partial r_c}{\partial p^b_{c}}=R^c_b(R^{b_j}_wR^w_{b_i}-I)

\small \frac{\partial r_c}{\partial q^b_{c}}=\lim_{\delta \theta \rightarrow 0}\frac{[R^b_cExp(\delta \theta )]^{-1}\left \{R^{b_j}_w[R^w_{b_i}(R^b_cExp(\delta \theta )\frac{1}{\lambda }\hat{P}^{C_i}_l+p^b_c)+p^w_{b_i}-p^w_{b_j}] -p^b_c\right \}-\left \{\cdot \right \}}{\delta \theta}

\small \approx \lim_{\delta \theta \rightarrow 0}\frac{(I-\delta \theta ^\wedge )R^c_b\left \{R^{b_j}_w[R^w_{b_i}(R^b_c(I+\delta \theta ^\wedge )\frac{1}{\lambda }\hat{P}^{C_i}_l+p^b_c)+p^w_{b_i}-p^w_{b_j}] -p^b_c\right \}-\left \{\cdot \right \}}{\delta \theta}

\small \approx \lim_{\delta \theta \rightarrow 0}\frac{-\delta \theta ^\wedge R^c_b\left \{R^{b_j}_w[R^w_{b_i}(R^b_c(I+\delta \theta ^\wedge )\frac{1}{\lambda }\hat{P}^{C_i}_l+p^b_c)+p^w_{b_i}-p^w_{b_j}] -p^b_c\right \}+R^c_b\left \{R^{b_j}_w[R^w_{b_i}(R^b_c\delta \theta ^\wedge \frac{1}{\lambda }\hat{P}^{C_i}_l+p^b_c)+p^w_{b_i}-p^w_{b_j}] -p^b_c\right \}}{\delta \theta}

\small =\left \{ R^c_b\left \{R^{b_j}_w[R^w_{b_i}(R^b_c\frac{1}{\lambda }\hat{P}^{C_i}_l+p^b_c)+p^w_{b_i}-p^w_{b_j}] -p^b_c\right \}\right \}^\wedge +R^c_b{R^{b_j}_wR^w_{b_i}R^b_c (\frac{\hat{P}^{C_i}_l}{\lambda})^\wedge

=(R^c_bR^{b_j}_wR^w_{b_i}R^b_c\frac{\hat{P}^{C_i}_l}{\lambda })^\wedge + \left \{ R^c_b[R^{b_j}_w(R^w_{b_i}p^b_c+p^w_{b_i}-p^w_{b_j}) -p^b_c]\right \}^\wedge+R^c_b{R^{b_j}_wR^w_{b_i}R^b_c (\frac{\hat{P}^{C_i}_l}{\lambda})^\wedge

\small \frac{\partial r_c}{\partial\lambda }=-R^c_bR^{b_j}_wR^w_{b_i}R^b_c\frac{\hat{P}^{C_i}_l}{\lambda ^2}

其中

\small (\cdot )=(R^b_c\frac{1}{\lambda }\hat{P}^{C_i}_l+p^b_c)

\small \left \{ \cdot \right \}=R^c_b\left \{R^{b_j}_w[R^w_{b_i}(R^b_c\frac{1}{\lambda }\hat{P}^{C_i}_l+p^b_c)+p^w_{b_i}-p^w_{b_j}] -p^b_c\right \}

至此视觉因子中的残差,以及残差关于状态量的雅克比矩阵推导完毕。最后为了与代码中的雅可比矩阵保持一致,各残差相对于状态量的雅可比可以整理为:

jacobian[0]=[\frac{\partial r_c}{\partial p^w_{b_i}},\frac{\partial r_c}{\partial q^w_{b_i}}]_{3\times 7}=[R^c_bR^{b_j}_w,-R^c_bR^{b_j}_wR^w_{b_i}(\cdot )^{\wedge }]

jacobian[1]=[\frac{\partial r_c}{\partial p^w_{b_j}},\frac{\partial r_c}{\partial q^w_{b_j}}]_{3\times 7}=[-R^c_bR^{b_j}_w,R^c_b\left \{[R^w_{b_j}]^{-1}[R^w_{b_i}(\cdot )+p^w_{b_i}-p^w_{b_j}]\right \}^{\wedge }]

jacobian[2]=[\frac{\partial r_c}{\partial p^b_{c}},\frac{\partial r_c}{\partial q^b_{c}}]_{3\times 7}=[R^c_b(R^{b_j}_wR^w_{b_i}-I),(R^c_bR^{b_j}_wR^w_{b_i}R^b_c\frac{\hat{P}^{C_i}_l}{\lambda })^\wedge + \left \{ R^c_b[R^{b_j}_w(R^w_{b_i}p^b_c+p^w_{b_i}-p^w_{b_j}) -p^b_c]\right \}^\wedge+R^c_b{R^{b_j}_wR^w_{b_i}R^b_c (\frac{\hat{P}^{C_i}_l}{\lambda})^\wedge]

jacobian[3]=[\frac{\partial r_c}{\partial\lambda }]_{3\times 1}=[-R^c_bR^{b_j}_wR^w_{b_i}R^b_c\frac{\hat{P}^{C_i}_l}{\lambda ^2}]

2.3、边缘化因子

这里首先说说什么是边缘化,边缘化就是在进行图优化的时候,为了保持实时性,对优化变量的维度控制在一定的程度的操作,在VINS中对应的就是滑动窗口的处理。既然是滑动窗口,就涉及到了对老旧图像帧的处理问题,最最直观的想法是每来一帧新的图像,就直接丢弃原来滑窗中最老的帧,然后对滑窗内的帧进行优化。这种朴素的做法显然会导致这样一个问题:没有了第一帧的约束,那我们只能约束滑窗内每帧之间的相对位姿,而不能约束它们的绝对位姿(相对[0,0,0]的位姿)因此,进行边缘化时我们需要将丢弃的图像帧也作为一个约束项,加入到我们的优化问题中,这也是VINS里面对边缘化的处理方法,将边缘化掉的帧作为先验约束加入到损失函数中进行计算。

在理解边缘化的时候,我们需要把握边缘化的核心是什么?边缘化的作用是什么?我的理解是边缘化一方面控制优化维度,保持计算的实时性;另一方面,它相当于对现在还在滑窗中的优化变量进行约束,不至于让滑窗中优化前后的数值相差太远。可以理解为,被边缘化的变量已经固定了,然后我们基于这些已经固定的量去优化滑窗里的内容。所以在实际实现的时候,首先我们需要找到需要边缘化的帧,然后找出该帧包含哪些因子,然后找到这些和这些因子相关联的优化变量,然后再对这些优化变量构建残差项,这大体就是VINS在边缘化上实现的总体逻辑。

VINS当中边缘化有两种形式,分别是MARGIN_OLD和MARGIN_SECOND_NEW,对应触发逻辑是:当前后帧追踪的特征较少或前后帧视差较大时进行MARGIN_OLD的操作,否则进行MARGIN_SECOND_NEW。两个不同的操作对应边缘化的过程也不同,具体为:

  1. MARGIN_OLD是边缘化滑窗中最老的一帧,找到最老帧相关的因子进行边缘化处理。
  2. MARGIN_SECOND_NEW则是丢弃当前帧的前一帧,并将预积分的结果叠加到当前帧上。

这里以MARGIN_OLD为例介绍整个边缘化的过程

  1. 记录last_marginalization_info中当前滑窗中第一帧的位姿和速度;
  2. 记录当前滑窗中第一帧的位姿和速度;
  3. 记录当前滑窗中第一帧看到的特征;
  4. 对之前找到的位姿、速度、特征等进行多线程边缘化,具体方法是舒尔补,求解出新的H矩阵和b矩阵,这里记为H^*_{0}​​和b^*_{0};
  5. H^*_{0}b^*_{0}进行分解,求出等价的雅克比J_L和残差e_p​​: 

H^*_0=J^T_L\cdot J_L

b^*_0=J^T_L\cdot e_p

J_L 的求解比较方便,可以直接对H^*_{0} 进行SVD分解获得,而对于残差,由于状态量发生变化时b^*_0也会发生变化,因此需要推导b^*_0的一般形式:

b^*=b^*_0+\frac{\partial b}{\partial x}dx=b^*_0+J^T_L\frac{\partial e_p}{\partial x}=b^*_0+J^T_LJ_Ldx=b^*_0+H_0dx

因此

e_p=(J^T_L)^{-1}\cdot (b^*_0+H_0dx)=(J^T_L)^{-1}\cdot b^*_0+J_Ldx

其中dx为当前状态量x与计算雅克比时的线性化点x_0​​的差。计算出等价雅克比和新的残差后,就可以交给ceres求解器进行计算,求出新的状态增量了。

需要补充的是,第五步求解等价雅克比和残差不是同步进行的,因为残差的变化需要在下一帧图像到来并进行优化后才会发生变化,因此代码里的做法是计算当前帧的等价雅克比J_L 和记录e_0=(J^T_L)^{-1}\cdot b^*_0,下一次优化时再计算新的残差e_p

边缘化是SLAM中比较重要且较难理解的部分,虽然核心相同,都是为了将求解联合分布替换为求解条件概率分布,但是不同的开源算法都有不同的实现方式,而且其中还涉及到FEJ问题,由于篇幅有限,这里就不再深入具体的展开,后续会考虑再针对SLAM中的边缘化问题再开一篇文章深入讨论,感兴趣的同学可以参考论文[2]和[3][4]中关于边缘化的解析。

2.4、同步时间标定

虽然这一部分的内容没有直接出现在VINS的文章中,但也是VINS实现中非常重要的部分,以至于在VINS_Fusion中作为默认的状态量进行优化。这一部分的工作是秦通在[3]中的主要内容,这篇论文也是IROS2018的Best Student Paper,可见其重要程度。

在具体推导相关的雅可比矩阵前,先介绍一下同步时间的概念。通常的VIO系统主要包含相机和IMU两种传感器,一般我们都会运用各自的时间戳来对image data和IMU data做时间对齐,然后再进行后续的计算。但是事实上,相机在实际曝光、获取数据的时刻和数据打上时间戳的时刻是不一致的,其中可能相差着几毫秒到几十毫秒不等,而此时运用相机数据的时间戳去和IMU进行时间对齐就会引入误差,我们可以通过图2来辅助理解。图的上半部分是相机真实的采样时间,而下半部分则是相机数据的打标时间,可以看到其中相差了一个t_d时间,又由于IMU数据往往能达到100HZ以上的频率,因此一个t_d时间内会相差几个乃至几十个IMU数据,若此时系统的运动非常剧烈,则很容易恶化Tracking的结果。

 图2

需要计算出t_d,就要把它引入到残差方程中,然后再推导其对应的雅可比矩阵即可。作者首先假设在td时间内相机速度是一定的,即

V^k_l=(\begin{bmatrix} u^{k+1}_l\\v^{k+1}_l \end{bmatrix}-\begin{bmatrix} u^{k}_l\\v^{k}_l \end{bmatrix})/(t_{k+1}-t_k)

其中V^K_l代表第k时刻第l个特征点的速度。如果数据实际采集时间和打标时间存在一个时间误差t_d,且在t_d时间内匀速运动,我们可以理解为实际的特征点的位置需要在原来位置的基础上增加一个像素距离d:

d=V^k_lt_d

 因此重投影误差可以重写为

e^k_l=z^k_l(t_d)-\pi({R^w_{c_k}}^T(P_l-p^w_{c_k}))

z^k_l=\begin{bmatrix} u^k_l\\v^k_l \end{bmatrix}+V^k_l\cdot t_d

其中e^k_l为k时刻第l个特征的重投影误差,[R^w_{c_k},p^w_{c_k}]为第k时刻的相机位姿。对于采用逆深度来参数化特征点的重投影误差可以表示为

e^k_l=z^k_l(t_d)-\pi({R^w_{c_k}}^T(\frac{1}{\lambda }z^{k+1}_l(t_d)-p^w_{c_k}))

z^k_l=\begin{bmatrix} u^k_l\\v^k_l \end{bmatrix}+V^k_l\cdot t_d

z^{k+1}_l=\begin{bmatrix} u^{k+1}_l\\v^{k+1}_l \end{bmatrix}+V^{k+1}_l\cdot t_d

将上式残差方程中(并不是直接代入),并推导重投影误差相对同步时间t_d的雅可比,有

\frac{\partial r_c}{\partial t_d}=\frac{\partial p^{C_j}_l}{\partial \hat{p}^{C_i}_l}\cdot \frac{\partial \hat{p}^{C_i}_l}{\partial t_d}-\frac{\partial \hat{p}^{C_j}_l}{\partial t_d}=R^c_bR^{b_j}_wR^w_{b_i}R^b_c\frac{1}{\lambda }\cdot V^i_l-V^j_l

3、代码实现

这里直接展示各个因子对应的雅可比和残差的计算部分

  • 视觉因子
bool ProjectionTdFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    TicToc tic_toc;
    // 取出待优化状态量
    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

    Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);

    Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

    double inv_dep_i = parameters[3][0];

    double td = parameters[4][0];

    Eigen::Vector3d pts_i_td, pts_j_td;
    // 运用t_d对特征点的像素坐标进行修正
    pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i;
    pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j;
    // 坐标变换,目的是从第i帧特征点转换到第j帧
    Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; // 转换到第i帧相机坐标系
    Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; // 转换到第i帧IMU坐标系
    Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; // 转换到世界坐标系
    Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); // 转换到第j帧IMU坐标系
    Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); // 转换到第j帧相机坐标系
    Eigen::Map residual(residuals);

#ifdef UNIT_SPHERE_ERROR 
    residual =  tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); //论文中的重投影误差
#else
    double dep_j = pts_camera_j.z();
    residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); // 重投影误差
#endif

    residual = sqrt_info * residual;

    if (jacobians)
    {
        Eigen::Matrix3d Ri = Qi.toRotationMatrix();
        Eigen::Matrix3d Rj = Qj.toRotationMatrix();
        Eigen::Matrix3d ric = qic.toRotationMatrix();
        Eigen::Matrix reduce(2, 3);
#ifdef UNIT_SPHERE_ERROR
        double norm = pts_camera_j.norm();
        Eigen::Matrix3d norm_jaco;
        double x1, x2, x3;
        x1 = pts_camera_j(0);
        x2 = pts_camera_j(1);
        x3 = pts_camera_j(2);
        norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3),            - x1 * x3 / pow(norm, 3),
                     - x1 * x2 / pow(norm, 3),            1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3),
                     - x1 * x3 / pow(norm, 3),            - x2 * x3 / pow(norm, 3),            1.0 / norm - x3 * x3 / pow(norm, 3);
        reduce = tangent_base * norm_jaco;
#else
        reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j),
            0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j);
#endif
        reduce = sqrt_info * reduce;

        if (jacobians[0])
        {
            Eigen::Map> jacobian_pose_i(jacobians[0]);

            Eigen::Matrix jaco_i;
            jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose(); // 对应公式4
            jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i); // 对应公式6

            jacobian_pose_i.leftCols<6>() = reduce * jaco_i;
            jacobian_pose_i.rightCols<1>().setZero();
        }

        if (jacobians[1])
        {
            Eigen::Map> jacobian_pose_j(jacobians[1]);

            Eigen::Matrix jaco_j;
            jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose(); // 对应公式5
            jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j);// 对应公式7

            jacobian_pose_j.leftCols<6>() = reduce * jaco_j;
            jacobian_pose_j.rightCols<1>().setZero();
        }
        if (jacobians[2])
        {
            Eigen::Map> jacobian_ex_pose(jacobians[2]);
            Eigen::Matrix jaco_ex;
            jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity()); // 对应公式8
            Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric;
            jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) +
                                     Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic)); // 对应公式9
            jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;
            jacobian_ex_pose.rightCols<1>().setZero();
        }
        if (jacobians[3])
        {
            Eigen::Map jacobian_feature(jacobians[3]);
            jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i); // 对应公式10
        }
        if (jacobians[4])
        {
            Eigen::Map jacobian_td(jacobians[4]);
            jacobian_td = reduce * ric.transpose() * Rj.transpose() * Ri * ric * velocity_i / inv_dep_i * -1.0  +
                          sqrt_info * velocity_j.head(2); // 对应公式22
        }
    }
    sum_t += tic_toc.toc();

    return true;
}
  • 预积分因子

以下代码中的公式对应这篇文章VINS系列:(二)IMU预积分详细推导_Derrr...的博客-CSDN博客

virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
    {
        // 取出优化变量
        Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
        Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

        Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
        Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);
        Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);

        Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);
        Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

        Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);
        Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);
        Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);

#if 0
        if ((Bai - pre_integration->linearized_ba).norm() > 0.10 ||
            (Bgi - pre_integration->linearized_bg).norm() > 0.01)
        {
            pre_integration->repropagate(Bai, Bgi);
        }
#endif
        // 构建IMU残差residual
        Eigen::Map> residual(residuals);
        residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                            Pj, Qj, Vj, Baj, Bgj);

        // LLT分解,residual 还需乘以信息矩阵的sqrt_info
        // 因为优化函数其实是d=r^T P^-1 r ,P表示协方差,而ceres只接受最小二乘优化
        // 因此需要把P^-1做LLT分解,使d=(L^T r)^T (L^T r) = r'^T r
        Eigen::Matrix sqrt_info = Eigen::LLT>(pre_integration->covariance.inverse()).matrixL().transpose();
        residual = sqrt_info * residual;

        if (jacobians)
        {
            // 获取预积分的误差递推函数中pqv关于ba、bg的Jacobian
            double sum_dt = pre_integration->sum_dt;
            Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA);
            Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG);

            Eigen::Matrix3d dq_dbg = pre_integration->jacobian.template block<3, 3>(O_R, O_BG);

            Eigen::Matrix3d dv_dba = pre_integration->jacobian.template block<3, 3>(O_V, O_BA);
            Eigen::Matrix3d dv_dbg = pre_integration->jacobian.template block<3, 3>(O_V, O_BG);

            if (pre_integration->jacobian.maxCoeff() > 1e8 || pre_integration->jacobian.minCoeff() < -1e8)
            {
                ROS_WARN("numerical unstable in preintegration");
                //std::cout << pre_integration->jacobian << std::endl;
///                ROS_BREAK();
            }

            // 第i帧的IMU位姿 pbi、qbi
            if (jacobians[0])
            {
                Eigen::Map> jacobian_pose_i(jacobians[0]);
                jacobian_pose_i.setZero();

                jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix(); // 对应公式(17)
                jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt)); // 对应公式(21)

#if 0
            jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();
#else
                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>(); // 对应公式(32)
#endif

                jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi)); // 对应公式(27)

                jacobian_pose_i = sqrt_info * jacobian_pose_i;

                if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8)
                {
                    ROS_WARN("numerical unstable in preintegration");
                    
                }
            }
            // 第i帧的imu速度vbi、bai、bgi
            if (jacobians[1])
            {
                Eigen::Map> jacobian_speedbias_i(jacobians[1]);
                jacobian_speedbias_i.setZero();
                jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt; // 对应公式(19)
                jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba; // 对应公式(22)
                jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg; // 对应公式(23)

#if 0
            jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg; 
#else
                
                jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * pre_integration->delta_q).bottomRightCorner<3, 3>() * dq_dbg; // 对应公式(35)
#endif

                jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix(); // 对应公式(25)
                jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba; // 对应公式(30)
                jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg; // 对应公式(29)

                jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();

                jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();

                jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;

            }
            // 第j帧的IMU位姿 pbj、qbj
            if (jacobians[2])
            {
                Eigen::Map> jacobian_pose_j(jacobians[2]);
                jacobian_pose_j.setZero();

                jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix(); // 对应公式(18)

#if 0
            jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();
#else
                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>(); // 对应公式(33)
#endif

                jacobian_pose_j = sqrt_info * jacobian_pose_j;

            }
            // 第j帧的IMU速度vbj、baj、bgj
            if (jacobians[3])
            {
                Eigen::Map> jacobian_speedbias_j(jacobians[3]);
                jacobian_speedbias_j.setZero();

                jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix(); // 对应公式(26)

                jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();

                jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();

                jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;

            }
        }

        return true;
    }
  • 边缘化因子
bool MarginalizationFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    
    int n = marginalization_info->n;
    int m = marginalization_info->m;
    Eigen::VectorXd dx(n);
    for (int i = 0; i < static_cast(marginalization_info->keep_block_size.size()); i++)
    {
        int size = marginalization_info->keep_block_size[i];
        int idx = marginalization_info->keep_block_idx[i] - m;
        Eigen::VectorXd x = Eigen::Map(parameters[i], size);
        Eigen::VectorXd x0 = Eigen::Map(marginalization_info->keep_block_data[i], size);
        if (size != 7)
            dx.segment(idx, size) = x - x0;
        else
        {
            dx.segment<3>(idx + 0) = x.head<3>() - x0.head<3>();
            dx.segment<3>(idx + 3) = 2.0 * Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
            if (!((Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).w() >= 0))
            {
                dx.segment<3>(idx + 3) = 2.0 * -Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
            }
        }
    }
    Eigen::Map(residuals, n) = marginalization_info->linearized_residuals + marginalization_info->linearized_jacobians * dx; // 对应公式(14)
    if (jacobians)
    {

        for (int i = 0; i < static_cast(marginalization_info->keep_block_size.size()); i++)
        {
            if (jacobians[i])
            {
                // 雅可比不变
                int size = marginalization_info->keep_block_size[i], local_size = marginalization_info->localSize(size);
                int idx = marginalization_info->keep_block_idx[i] - m;
                Eigen::Map> jacobian(jacobians[i], n, size);
                jacobian.setZero();
                jacobian.leftCols(local_size) = marginalization_info->linearized_jacobians.middleCols(idx, local_size);
            }
        }
    }
    return true;
}

4、反思与探讨

5、参考文献

[1] Qin T ,  Li P ,  Shen S . VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator[J]. IEEE Transactions on Robotics, 2018.

[2] Sibley G ,  Matthies L ,  Sukhatme G . Sliding window filter with application to planetary landing[J]. Journal of Field Robotics, 2010, 27.

[3] Qin T, Shen S. Online Temporal Calibration for Monocular Visual-Inertial Systems[J]. arXiv preprint arXiv:1808.00692, 2018.

[4] VINS 论文推导及代码解析. 崔华坤

[5] 白话VINS-Mono之边缘化(五) - 知乎 (zhihu.com)

[6] VIO系统中IMU与相机时间偏差标定(PaperReading) - 知乎 (zhihu.com)

你可能感兴趣的:(SLAM,计算机视觉,slam,c++,opencv,自动驾驶)