VINS-Fusionの初期化およびオンライン外力パラメータ標定において、連続する2つの画像フレームから相対的な回転行列を推定するプロセスは極めて重要です。ここでは、特徴点のマッチング情報を用いてカメラの姿勢変化を特定する手法について解説します。

2フレーム間の相対回転推定アルゴリズム

以下の関数 estimateRelativeRotation は、正規化された特徴点のペア集合を入力として受け取り、2つのフレーム間におけるカメラの回転行列 $R$ を計算します。

Matrix3d InitialExtrinsicRotation::estimateRelativeRotation(const vector> &matchPairs)
{
    // 安定した推定のために最低9点の対応が必要
    if (matchPairs.size() < 9)
    {
        return Matrix3d::Identity();
    }

    vector<cv::Point2f> ptsLeft, ptsRight;
    for (const auto &match : matchPairs)
    {
        ptsLeft.emplace_back(match.first(0), match.first(1));
        ptsRight.emplace_back(match.second(0), match.second(1));
    }

    // 基礎行列（Fundamental Matrix）の推定
    cv::Mat fundamentalMatrix = cv::findFundamentalMat(ptsLeft, ptsRight);
    
    cv::Mat_<double> rot1, rot2, trans1, trans2;
    // 基礎行列を分解し、回転（R）と並進（t）の候補（計4パターン）を取得
    decomposeE(fundamentalMatrix, rot1, rot2, trans1, trans2);

    // 回転行列の行列式を確認し、必要に応じて符号を反転
    if (cv::determinant(rot1) + 1.0 < 1e-09)
    {
        fundamentalMatrix = -fundamentalMatrix;
        decomposeE(fundamentalMatrix, rot1, rot2, trans1, trans2);
    }

    // 4つの姿勢候補に対して三角測量を行い、最も妥当な解を選択
    double score1 = std::max(checkTriangulation(ptsLeft, ptsRight, rot1, trans1), 
                             checkTriangulation(ptsLeft, ptsRight, rot1, trans2));
    double score2 = std::max(checkTriangulation(ptsLeft, ptsRight, rot2, trans1), 
                             checkTriangulation(ptsLeft, ptsRight, rot2, trans2));

    cv::Mat_<double> bestRotationCV = (score1 > score2) ? rot1 : rot2;

    // OpenCVのMat形式からEigenのMatrix3d形式に変換（転置を考慮）
    Matrix3d rotationEigen;
    for (int r = 0; r < 3; r++)
        for (int c = 0; c < 3; c++)
            rotationEigen(c, r) = bestRotationCV(r, c);

    return rotationEigen;
}

推定プロセスの詳細

1. 基礎行列の算出

cv::findFundamentalMat を用いて、2枚の画像間のエピポーラ幾何学的関係を示す基礎行列 $F$ を求めます。正規化座標系ではこれは本質的行列 $E$ に相当します。この行列は $p_2^T F p_1 = 0$ という拘束条件を満たします。

2. 行列分解による姿勢候補の抽出

基礎行列をSVD（特異値分解）することで、理論的には以下の4つの組み合わせが導き出されます。

• $(R_1, t_1), (R_1, t_2), (R_2, t_1), (R_2, t_2)$

これらの中から、幾何学的に正しい1つのペアを特定する必要があります。

3. 三角測量による検証（カイラリティ制約）

選択された回転と並進が正しいかどうかを判定するために、checkTriangulation 関数で点を3次元空間に投影します。正しい姿勢であれば、再投影された3次元点は両方のカメラの前方（深度が正）に位置するはずです。

double InitialExtrinsicRotation::checkTriangulation(const vector<cv::Point2f> &l, 
                                                    const vector<cv::Point2f> &r, 
                                                    const cv::Mat_<double> &R, 
                                                    const cv::Mat_<double> &t)
{
    // プロジェクション行列の構築
    cv::Matx34f projLeft(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0);
    cv::Matx34f projRight(R(0,0), R(0,1), R(0,2), t(0),
                          R(1,0), R(1,1), R(1,2), t(1),
                          R(2,0), R(2,1), R(2,2), t(2));

    cv::Mat pointCloud;
    cv::triangulatePoints(projLeft, projRight, l, r, pointCloud);

    int validCount = 0;
    for (int i = 0; i < pointCloud.cols; i++)
    {
        // 斉次座標から3次元座標への変換
        cv::Mat p4d = pointCloud.col(i);
        p4d /= p4d.at<float>(3);
        
        cv::Mat p3dPrev = cv::Mat(projLeft) * p4d;
        cv::Mat p3dCurr = cv::Mat(projRight) * p4d;

        // 両方のカメラで深度が正であることを確認
        if (p3dPrev.at<double>(2) > 0 && p3dCurr.at<double>(2) > 0)
        {
            validCount++;
        }
    }
    return static_cast<double>(validCount) / pointCloud.cols;
}

まとめ

この手法のフローは、特徴点ペアの取得から始まり、エピポーラ幾何に基づいた基礎行列の推定、そしてSVD分解による候補の生成を経て、最終的に物理的な整合性（カイラリティ制約）を確認することで、最適な回転行列を決定します。このプロセスは、IMUのバイアス推定やカメラ・IMU間の外力パラメータの動的な修正において、基準となる視覚的な動きを提供します。