point[][]初始化_vins-mono中的初始化

vins-mono采用了一个松耦合传感器融合方法来获得真值，通过将只基于视觉的sfm结果与米制IMU预积分对齐，可以粗略地估计尺度，重力，速度和甚至偏置。在本文中，初始化阶段忽略加速度偏置项，加速度偏置与重力耦合，由于相对于重力的幅度来说，加速度偏置是个很小的数了，很难观察到。

A、估计旋转外参数（具体算法在InitialEXRotation::CalibrationExRotation中）

通过九点法估计基本矩阵，得到第时刻之间的旋转矩阵，通过预积分得到两个时刻间的imu之间的旋转积分为，则可以得到以下的旋转约束：

上式可以写为：

其中：

将多个时刻的线性函数累计起来，并且加上鲁棒核权重，得到：

求解的终止条件需要窗口中有足够的帧而且需要的零空间的秩为1，判断它的秩为1的条件是它的第二小奇异值是否大于某个阈值，代码中的阈值为0.25。

B、滑动窗口基于视觉的SfM

1、目的：

由于现在还不知道世界帧的任何信息，本文将第一个相机帧作为SFM的参考帧。所有帧的位姿和特征点位置是相对于第0帧的相机。假设已经有了关于相机和IMU之间的测量外参数，可以将位姿从相机坐标系转换为（IMU）坐标系（有旋转约束和平移约束）：

2、具体方法：首先滑动窗口中的帧是满的，在最新一帧和窗口中的其它所有帧之间检查特征对应点。如果在最新一帧和在窗口中的其它帧之间找到了稳定的跟踪特征点（超过30个跟踪特征）和足够的视差（超过20个旋转补偿像素），则对这两帧恢复旋转和尺度位移。以上代码在Estimator::relativePose中。得到了这两帧的相对旋转和位移，对特征点进行三角化，通过三角化得到的路标点进行pnp操作估计在窗口中所有帧的位姿（代码在GlobalSFM::construct中），以及通过所有帧的位姿得到所有路标点，最后通过全局的Bundle Adjustment来最小化重投影误差。其中三角化的代码在GlobalSFM::triangulatePoint中，它的具体方法为：

• 三角化：

考虑某路标点y被若干个帧看到，，取齐次坐标。每次观测为，取归一化平面的坐标（这样可以忽略掉内参）。记投影矩阵，为world系到camera系，他们之间的投影关系为：

其中为第k帧的深度，为，为矩阵的第三行，将深度带入公式可得：

每次观测都可以提供这样的两个方程，可以将其合成为的形式，那么就是零空间的元素，所以将进行SVD分解得到的最后一维向量。具体代码如下所示：

void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
                        Vector2d &point0, Vector2d &point1, Vector3d &point_3d)
{
    Matrix4d design_matrix = Matrix4d::Zero();
    design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);
    design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);
    design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);
    design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);
    Vector4d triangulated_point;
    triangulated_point =
              design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
    point_3d(0) = triangulated_point(0) / triangulated_point(3);
    point_3d(1) = triangulated_point(1) / triangulated_point(3);
    point_3d(2) = triangulated_point(2) / triangulated_point(3);
}

C、Visual-Inertial Alignment（视觉和IMU进行对齐）

1、陀螺仪偏置的标定

考虑在窗口中的连续两帧

和

，前面已经通过pnp得到了两帧之间的位姿，然后将两帧之间的旋转通过sfm求出来

和

。论文中是相对于相机坐标系来说的，其中通过对IMU数据的预处理得到了相机两帧之间的相对旋转，也通过相机的sfm得到了两帧的旋转，需要让这两者的误差达到最小，即：

其中表示在滑动窗口中的所有图像帧。可以通过最小二乘法来进行求解，预积分的一阶泰勒近似为：

所以可以求取雅克比矩阵，构建正定方程即可求解，具体的代码见initial_aligment.cpp 函数的solveGyroscopeBias()中。在得到陀螺仪偏置之后，重新对IMU预积分

进行估计。

2、速度、重力和米制尺度初始化

在估计完陀螺仪偏置之后需要估计速度，重力和尺度因子，这些变量可以写成以下形式：

其中，

表示第k时刻的body坐标系下速度在取第k帧图像的时候，

表示重力向量在第0帧相机坐标系下的表示。s表示尺度因子，将视觉轨迹拉伸到米制单位。在世界坐标系下的有：

将其转换为以第0帧为坐标系的公式有：

将上式展开，并将B中的公式带入，得到：

将需要优化的量放在右边，与优化无关的量放在左边，得到：

在上式中

都可以从SFM中获得，

是两个图像帧之间的时间间隔，通过最小二乘法使下式最小化，就可以估计出速度，重力和尺度，

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

3、重力优化

需要对重力进行优化的原因主要是因为在之前的线性初始化过程中并没有对重力的幅度进行限制，所有重力只有两个自由度。在许多场合下，重力的幅度是已知的，因此可以重新参数化重力向量在切平面的两个变量，与重力正交的且在切平面上的两个向量

如下图所示：

如何找到可以通过以下算法找到：

其代码为

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

上面的过程其实是施密特正交化，将其转化为三个正交的向量，其中的一个向量[0,0,1]或者[1,0,0]是与地心对应的，那么剩余的两个向量刚好是地球的切平面。

重力向量被改写为

将与之前求出的

有关的变量移到方程的左边，再一次对

进行优化，将优化得到的结果相加（由于w只有两个值所以只需要2个变量）。它的左边项也发生了改变，变成了：

优化参数少了一个，因为多另一个模值的信息，hessian矩阵对于重力优化的部分会乘以[w1, w2]，这是个3*2的矩阵，所以优化变量就少了一个。

具体细节代码见：initial_alignment的RefineGravity。

4、完成初始化

重力优化后得到了在窗口中的第一帧的图像坐标系下的重力，之后就可以通过世界坐标系下的重力加速度和相机坐标系下的重力加速度之间的关系求解出世界坐标系与相机坐标系之间的旋转矩阵。通过

与

之间的叉乘可以得到两者的旋转轴，再除以两者叉乘的模长就能得到他们的单位旋转轴，他们的旋转角可以通过如下计算：

所以

在代码中

得到该旋转矩阵之后，可以将第一帧图像坐标系下的所有变量都转换为世界坐标系下的值，在窗口中的所有帧都设置成了关键帧。至此，完成了初始化。'

参考资料：

VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator . Tong Qin, Peiliang Li, and Shaojie Shen

深蓝学院vio课程