VINS-MONO初始化

Posted 2020-11-28 glxin

视觉和IMU的对齐

1. 求陀螺仪的零偏(b_g)

视觉计算出了帧间的旋转，IMU也计算出了帧间的旋转，这两者之间并不太一致，那么谁是对的呢？这里采用视觉计算出的值，认为IMU由于零偏的不准确导致计算的旋转不准确。通过调整零偏，使得目前所有帧的帧间旋转都尽量的靠近视觉计算的值。同时，这里还假设了零偏没有错的太离谱，所以IMU计算的帧间的旋转（也就是预积分的(gamma)）可以用它对零偏的一阶泰勒展开来表示。这样就可以构成一个超定方程组。求解这个方程组，就能够得到零偏增量的估计，加到原来的零偏上就获得了新的零偏。

滑窗中各帧已经有了一个up-to-scale的位姿。也有了以初始估计零偏求的的预积分的值。陀螺仪零偏估计，就是要估计出一个陀螺仪零偏，使得滑窗中各帧间由陀螺仪求得的(delta q)与视觉求得的(delta q)的误差最小。这是一个典型的最小二乘问题。
$$argmin_{b_g}sum_{kin B}Vert {q^{{c_0}_{b_{k+1}}}}{-1} otimes q^{c_0}{b_k} otimes q^{b_k}{b_{k+1}} Vert^2$$

另外有预积分的一阶泰勒近似：

$$q^{b_k}_{b_{k+1}} approx hat{q}^{b_k}_{b_{k+1}} otimes left [egin{smallmatrix}1 \ {1 over 2} J^q_{b_g}  delta b_gend{smallmatrix}
ight]$$

我们把视觉的测量值当做真值(q^{b_k}_{b_{k+1}})，把IMU预积分(gamma)值作为(hat{q}^{b_k}_{b_{k+1}})，这样就有：

[{hat{q}^{b_k}_{b_{k+1}}}^{-1} otimes q^{b_k}_{b_{k+1}} = left [egin{smallmatrix}1 \ {1 over 2} J^q_{b_g} delta b_gend{smallmatrix} ight] ]

利用它们的虚部相等就可以构建形如(Ax = b)的方程。带求解量是(delta b_g)。

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

具体代码见： initial_aligment.cpp 函数 solveGyroscopeBias()。

2.线性对齐求出各帧的速度 (v_k)，重力加速 (g)以及尺度 (s)

线性对齐就是用IMU预积分(alpha, eta)值来进行对齐。我们有

[egin{split} alpha^{b_i}_{b_j} & = & R^{b_i}_w(p^w_{b_j} - p^w_{b_i} - v_i^wDelta t+{1over 2}g^wDelta t^2) \ eta^{b_i}_{b_j} & = & R^{b_i}_w(v^w_j - v^w_i + g^wDelta t) end{split}]

可以看到它们其实是两个时刻距离及速度增量再加上重力加速度的影响。距离跟速度又是相关的。同时，两帧间的距离又可以通过视觉测量得到。这样它们就有了对齐的基础。
我们把上面的世界坐标系替换成(c_0)坐标系，并把(p^w_{b_i}, p^w_{b_j})用视觉测量量来代替。由于视觉测量是up-to-scale的，因此要带上尺度因子。这样就有：

[egin{split} alpha^{b_k}_{b_{k+1}} & = & R^{b_k}_{c_0}(s(ar{p}^{c_0}_{b_{k+1}}-ar{p}^{c_0}_{b_k})-R^{c_0}_{b_k}v^{b_k}Delta t+{1over 2}g^{c_0}Delta t^2) \beta^{b_k}_{b_{k+1}} & = & R^{b_k}_{c_0}(R^{c_0}_{b_{k+1}}v^{b_{k+1}}_{k+1} - R^{c_0}_{b_k}v^{b_k}_k+g^{c_0}Delta t) end{split}]

上式中的$ ar{p}^{c_0}{b_k}, ar{p}^{c_0}{b_{k+1}}(实际上还不是视觉测量， 视觉测量的是camera的距离)ar{p}^{c_0}{c_k}, ar{p}^{{c_0}_{c_{k+1}}$，它们与$ar{p}}{c_0}{b_k}, ar{p}^{{c_0}_{b_{k+1}}$之间可以通过相机与IMU的外参转换:$ar{p}}{c_0}{b_k} = ar{P}^{c_0}{c_k} - R^{c_0}_{b_k}Pb_c, quad ar{p}^{c_0}{b{k+1}} = ar{P}^{c_0}{c{k+1}} - R^{{c_0}_{b_{k+1}}P}b_c(，其中)P^b_c$表示的是在body坐标系下camera到IMU的距离。代入上式中整理可以得到：

[alpha^{b_k}_{b_{k+1}} = sR^{b_k}_{c_0}(ar{p}^{c_0}_{c_{k+1}}-ar{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}Delta t+{1over 2}R^{b_k}_{c_0}g^{c_0}Delta t^2 ]

将待估计变量放到方程右边，写成矩阵相乘的模式有：

[egin{split}hat{z}^{b_k}_{b_{k+1}} = left[ egin{matrix} hat{alpha}^{b_k}_{b_{k+1}} -p^b_c+R^{b_k}_{c_0}R^{c_0}_{b_{k+1}}P^b_c \ hat{eta}^{b_k}_{b_{k+1}}end{matrix} ight] = H^{b_k}_{b_{k+1}} mathcal{X}^{k}_I + n^{b_k}_{b_{k+1}}end{split} ]

其中：

[mathcal{X}^{k}_I = left[egin{matrix} v^{b_k}_{k}, v^{b_{k+1}}_{k+1}, g^{c_0}, send{matrix} ight]^T ]

是待估计的量。

[H^{b_k}_{b_{k+1}} = left[egin{matrix}-Delta t I_{3 imes 3} & 0_{3 imes 3} & {1over2}R^{b_k}_{c_0}Delta t^2 & R^{b_k}_{c_0}(ar{p}^{c_0}_{c_{k+1}}-ar{p}^{c_0}_{c_k}) \ -I_{3 imes 3} & R^{b_k}_{c_0}R^{c_0}_{b_{k+1}} & R^{b_k}_{c_0}Delta t & 0_{3 imes 1} end{matrix} ight]_{6 imes 10} ]

(n^{b_k}_{b_{k+1}})是噪声项。
这样就可以构建最小二乘问题。

[arg min_{mathcal{X}_I} sum_{k in mathcal{B}}Verthat{z}^{b_k}_{b_{k+1}} - H^{b_k}_{b_{k+1}}mathcal{X}^{k}_IVert^2 ]

这里的(sum)的处理需要注意一下。因为在(k)时刻的状态包括([v^{b_k}_k, v^{b_{k+1}}_{k+1}, g^{c_0}, s])， 每个时刻跟它的下一个时刻既有相交又有错位，它们在最终的大矩阵中的位置需要考虑清楚。假如窗口中有10个帧，其实我们可以把每个时刻的状态都是34维的，而不是10维的，多出的部分用零补足。比如(0)时刻(mathcal{X}^0_I = [v^{b_0}_0, v^{b_1}_1, 0, cdots , 0, g^{c_0}, s])，相应的(H^{b_0}_{b_1})就可以写成([A^{0}_{6 imes 6}, 0, 0, cdots, 0, A^{0}_{4 imes4}])。那么(H^TH)就是如下形式：

[left[egin{matrix} A_{6 imes 6} & 0 & cdots & 0 & A_{6 imes 4} & 0 & cdots & 0 & 0 \vdots & vdots & vdots & vdots & vdots A_{6 imes 4} & 0 & cdots & 0 & A_{4 imes4} end{matrix} ight]]

这样在大矩阵中的位置就很清楚了。每次时刻递推，(A_{6 imes 6})往右下移动(3)，(A_{6 imes 4})往下移动(3)， (A_{4 imes 6})往右移动(3)。

     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<4, 4>() += r_A.bottomRightCorner<4, 4>();
        b.tail<4>() += r_b.tail<4>();
        A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();
        A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();

具体代码见： initial_aligment.cpp 函数 LinearAlignment()。

3.重力向量的进一步优化

上面的到的重力向量无法保证它的模是9.8。因为在上一步中重力向量是一个无约束的3个自由度向量，而实际上重力向量(g)只有两个自由度，它的模是确定的。但是如果将(g)的约束加到上面的求解过程中，上面的线性优化问题就变成了非线性优化，增加了复杂度。因此，可以在上一步求出的结果上再次进行优化修正。

将重力向量参数化为

[hat{mathcal{g}}^{c_0} = 9.8 hat{ar{mathcal{g}}}^{c_0} + w_1 b_1+ w_2 b_2 ]

其中(b_1, b_2)是目前估计出的(g)切平面的上的一组单位正交基。
将这个重新参数化之后的(g^{c_0})代入到上一步求解的方程中，可以求的对应的(w_1, w_2)，反复迭代几次，最终会得到一个收敛的(g^{c_0})。

4.从(c_0)坐标系到世界坐标系的旋转

目前已经知道了(g^{c_0})，只要找到一个矩阵把(g^{c_0})旋转到(z)轴， 那么这个矩阵就能够把(c_0)系就与世界坐标系在(z)方向上对齐了。不过需要注意的是， IMU的测量量在静止的时候，除了零偏外，测得的是重力加速度的反向向量。根据前面(alpha^{b_k}_{b_{k+1}}, eta^{b_k}_{b_{k+1}})的计算公式，这里的(g^{c_0})其实是加速度的反向。因此这里是把坐标系的(z)轴与重力方向的反向对齐，也就是垂直向上。

Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g)
{
    Eigen::Matrix3d R0;
    Eigen::Vector3d ng1 = g.normalized();
    Eigen::Vector3d ng2{0, 0, 1.0};
    //R0将ng1旋转到[0,0,1]，也就相当于旋转坐标系把z轴旋转到了ng1
    R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix();
    double yaw = Utility::R2ypr(R0).x();
    //旋转的过程中可能改变了yaw角，再把yaw旋转回原来的位置。
    //这个是沿z轴的旋转，因此不改变g沿z轴的方向。只是坐标系的x,y轴改变了。
    R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
    // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0;
    return R0;
}

上面这一步之后，经过(R_0)修正，参考坐标系(z)轴向下，(x)轴是(c_0)的前方。不过需要注意的是前面视觉在计算位姿的流程中我们找到与窗口中第一个与滑窗中最后一帧有足够视差的帧建立参考系，这里的(c_0)实际上不一定是第0帧，它在某些情况下可能是第(x)帧(c_x)。因此，我们要使参考坐标系的(x)轴朝向真正的第(0)帧的水平正前方。

    // R0将参考坐标系旋转到z轴垂直向上。
    Matrix3d R0 = Utility::g2R(g);
    // R0将参考系的y轴旋转到第0帧的IMU正前方，这个时候x轴也确定了向右。
    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;
    //目前的Ps,Rs,Vs都是在$c_x$系下的表达，将它们转换到参考坐标系。
    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];
    }