vins-mono视觉惯导对齐

VINS-Mono视觉惯性对齐包括三步：

        1）陀螺仪偏置矫正

        2）速度、重力向量、尺度因子初始化

        3）重力修正

c0代表视觉坐标系第0帧，bk代表body坐标系第k帧

（6）

（6）式变为：

（7）

并且已知：

（7）式变为：

所以：

为了能够方便的利用LDLT方法对上式求解，

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; //所有状态{v_b0_b0, v_b1_b1...v_bn_bn, s, g_c0}的数量

    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); // 公式中的b，也即z_b(k+1)_b(k)
        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)和(H)T(b)
        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>();
    }
    A = A * 1000.0;
    b = b * 1000.0;
    x = A.ldlt().solve(b);             // 使用LDLT分解法计算状态量
    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());
    // 如果优化后获得的重力加速度模与设定的重力加速度模差1m/s2，或者尺度因子小于0，则认为视觉惯性对齐失败
    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;
}

因在上面优化过程中，g也是变量，数值会变化，所以需要修正。

如图，重力矢量大小固定，半球半径即为模长，取切平面上相互垂直的b1、b2方向，便可与重力方向组成三维正交基，实现在三维空间中将重力参数化。

下面就简单了，与第二步相同，用矩阵分解计算。