SLAM--本质矩阵的分解

本质矩阵是归一化图像坐标下的基本矩阵的特殊形式:
$$x_2^{T}E{x}_1=0$$

矩阵分解

• =本质矩阵的性质是它的奇异值有两个相等而第三个是０
  设：
  正交矩阵W:
  $$W=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$$
  反对称阵z:
  $$Z=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]$$
  $E=t^{\wedge }R=SR$，其中Ｓ是反对称矩阵
  根据定义：
  $$S=kUZ{U}^T$$
  其中，Ｕ是正交矩阵，Ｚ＝diag(1,1,0)W
  得到:
  $$S=kUdiag(1,1,0)W{U}^T$$
  即：
  $$E=Udiag(k,k,0)W{U}^{T}R$$
  由SVD分解可得：
  $$W{U}^{T}R=V^T$$
  由于E和-E等价则有两种可能：
  $$\begin{gathered} R=U{W}^T{V}^T \\ =U(-W^T)V^T \\ =UW{V}^T \end{gathered}$$
  又因为t^=S，忽略k，化简后得到t等于U的最后一列，此时也有两种可能，故总共四种可能，这是需要手性验证，找到特征点在两个相机位姿下的深度均为正的解。

vins代码

Matrix3d InitialEXRotation::solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres)
{
    if (corres.size() >= 9)
    {
        vector<cv::Point2f> ll, rr;
        for (int i = 0; i < int(corres.size()); i++)
        {
            ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
            rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
        }

        //求解两帧的本质矩阵
        cv::Mat E = cv::findFundamentalMat(ll, rr);
        cv::Mat_<double> R1, R2, t1, t2;
        
        //本质矩阵svd分解得到4组Rt解
        decomposeE(E, R1, R2, t1, t2);

        if (determinant(R1) + 1.0 < 1e-09)
        {
            E = -E;
            decomposeE(E, R1, R2, t1, t2);
        }

        //通过三角化得到的正深度选择Rt解
        double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2));
        double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2));
        cv::Mat_<double> ans_R_cv = ratio1 > ratio2 ? R1 : R2;

        //对R求转置
        Matrix3d ans_R_eigen;
        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 3; j++)
                ans_R_eigen(j, i) = ans_R_cv(i, j);
        return ans_R_eigen;
    }
    return Matrix3d::Identity();
}
void InitialEXRotation::decomposeE(cv::Mat E,
                                 cv::Mat_<double> &R1, cv::Mat_<double> &R2,
                                 cv::Mat_<double> &t1, cv::Mat_<double> &t2)
{
    cv::SVD svd(E, cv::SVD::MODIFY_A);
    cv::Matx33d W(0, -1, 0,
                  1, 0, 0,
                  0, 0, 1);
    cv::Matx33d Wt(0, 1, 0,
                   -1, 0, 0,
                   0, 0, 1);
    R1 = svd.u * cv::Mat(W) * svd.vt;
    R2 = svd.u * cv::Mat(Wt) * svd.vt;
    t1 = svd.u.col(2);
    t2 = -svd.u.col(2);
}