vins estimator InitialEXRotation

• CalibrationExRotation

  • 功能：若未提供外参时，该类可以帮你校准imu与相机的外参(旋转)

  • 参数：两帧图像对应的特征点，imu的旋转，输出外参矩阵

  • 原理：

    • $R_{cb}{R}_{b_k{b}_{k+1}{R}_{cb}^T}=R_{c_k{c}_{k+1}}$
    • $R_{cb}{R}_{b_k}{R}_{b_{k+1}}=R_{c_k{c}_{k+1}}{R}_{cb}$
    • $q_{cb}\otimes q_{b_k{b}_{k+1}}=q_{c_k{c}_{k+1}}\otimes q_{cb}$
    • 用到 四元数的左右乘法 $p\otimes q=[p{]}_{L}q=[q{]}_{R}p$
    • $([q_{c_k{c}_{k+1}}{]}_L-[q_{b_k{b}_{k+1}}{]}_R)q_{cb}=0$

  • 代码实现：

    • 每调用一次该函数，则 记录一次 imu旋转与 图像帧旋转之差

    • 1、根据两帧特征点求解两帧的旋转矩阵 --帧间cam的R，由对极几何得到

      • 1.1、根据两个图像中的相应点计算基本矩阵。findFundamentalMat
      • 1.2、本质矩阵svd分解得到四组RT的解 decomposeE
      • 1.3、通过三角化得到的正深度选择Rt解
      • 求解当前R,t下特征点的正深度比例 testTriangulation
        • 输入：ll+rr+R+t
        • 两帧间的相对关系，故$P_0=I$，$P_1=[R,t]$
        • 输出三角化后的特征点的3D坐标 cv::triangulatePoints(P, P1, l, r, pointcloud)
          • 两个相机位姿和特征点在两个相机坐标系下的坐标
          • 输出的3D坐标是齐次坐标，共四个维度，因此需要将前三个维度除以第四个维度以得到非齐次坐标xyz
        • 将每个特征点还原3D（齐次转非齐次），并统计深度＞0的个数
        • 返回 深度大于0的个数的比例
      • 得到 正深度比例最大的 R
      • 1.4、对 R 求转置并 return

    • 2、帧间IMU的旋转(由IMU预积分得到)，基于预估外参将其转换到激光坐标系，并将三个R存储起来

      • 3、遍历所有frame，每一帧分别计算：
        • 计算 相机对极几何与IMU预积分 角度差 angular_distance = $p^{T}q$
        • 计算鲁棒核函数 huber = dist > 5 ? 5/dist : 1;
        • 计算 相机帧间旋转的四元数左乘
        • 计算 imu预积分旋转的四元数右乘
        • 二者相减，同时乘以损失函数，即可得到 $s([q_{c_k{c}_{k+1}}{]}_L-[q_{b_k{b}_{k+1}}{]}_R)q_{cb}=0$

    • 4、svd分解中最小奇异值对应的右奇异向量作为旋转四元数

      • svd 分解： JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
      • 得到最小特征向量：Quaterniond estimated_R(svd.matrixV().col(3));
      • 至少迭代计算了WINDOW_SIZE次，且R的奇异值大于0.25才认为标定成功
        • R取特征值：Vector3d ric_cov = svd.singularValues().tail<3>()
        • 需要最大特征值大于 0.25 ric_cov(1) > 0.25
        • 结果为：estimated_R.toRotationMatrix().inverse()
        • 并返回 true

    • 否则返回false