Vins-Fusion之相机—IMU在线标定（两帧间旋转估计）（十）

重点理解以下几点：

a.程序的过程

b.包含的数据计算公式(暂时未完全明白，还要理解：本质矩阵分解R/T、三角化验证)

c.怎么得到最优解？

在函数solveRelativeR(corres) 得到的是视觉两帧的相对旋转，具体代码如下：

cpp 复制代码

//从一对帧之间的归一化特征匹配点集 corres 中，估计这两帧相机之间的相对旋转矩阵
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);//估计基础矩阵E,用于后续分解得到相对旋转矩阵R1和R2
        cv::Mat_<double> R1, R2, t1, t2;
        decomposeE(E, R1, R2, t1, t2);//从 E 分解出 两个可能的旋转 R1, R2 和两个平移 t1, t2，共有 4 种姿态组合

        if (determinant(R1) + 1.0 < 1e-09)
        {
            E = -E;
            decomposeE(E, R1, R2, t1, t2);
        }
        //四种组合分别做三角化测试
        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;//选择三角化测试得分更高的组合作为最终的相对旋转矩阵

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

输入：两帧之间的特征匹配点对

输出：两帧相机之间的相对旋转矩阵 R

步骤1：提取归一化坐标

cpp 复制代码

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

步骤2：估计基础矩

cpp 复制代码

cv::Mat E = cv::findFundamentalMat(ll, rr);

从两幅图像中的对应点计算基本矩阵。

cv::findFundamentalMat 是 OpenCV 中用于计算两幅图像之间基本矩阵（Fundamental Matrix）的函数。基本矩阵描述了两个未校准摄像机之间的几何关系，它在计算机视觉中用于立体视觉、运动结构恢复（Structure from Motion, SfM）、视觉里程计等任务。

基础矩阵 FF（代码中变量名为 E）满足：p2TFp1=0

从匹配点对估计 F

步骤3：分解基础矩阵得到候选姿态

cpp 复制代码

cv::Mat_<double> R1, R2, t1, t2;
decomposeE(E, R1, R2, t1, t2);

数学原理：

从基础矩阵 FF 可分解出本质矩阵 E（归一化坐标下）

本质矩阵 E= $t$ ×RE，其中 $t$ × $t$ × 是平移的反对称矩阵

SVD 分解 E=UΣVT 后，可构造两个旋转和两个平移方向

结果：4 种候选姿态组合

(R1, t1)

(R1, t2)

(R2, t1)

(R2, t2)

步骤4：三角化测试选择正确解

cpp 复制代码

double InitialEXRotation::testTriangulation(...)
{
    // 1. 构建两个相机的投影矩阵
    cv::Matx34f P = cv::Matx34f(1, 0, 0, 0,   // 第一帧：单位矩阵（参考帧）
                                 0, 1, 0, 0,
                                 0, 0, 1, 0);
    cv::Matx34f P1 = cv::Matx34f(R(0,0), R(0,1), R(0,2), t(0),  // 第二帧：R|t
                                  R(1,0), R(1,1), R(1,2), t(1),
                                  R(2,0), R(2,1), R(2,2), t(2));
    
    // 2. 三角化得到3D点云
    cv::triangulatePoints(P, P1, l, r, pointcloud);
    
    // 3. 统计在两相机前方（深度>0）的点数
    int front_count = 0;
    for (int i = 0; i < pointcloud.cols; i++)
    {
        // 将齐次坐标转为3D坐标
        cv::Mat_<double> p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor);
        cv::Mat_<double> p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor);
        
        // 检查深度是否为正（点在相机前方）
        if (p_3d_l(2) > 0 && p_3d_r(2) > 0)
            front_count++;
    }
    
    // 4. 返回前方点的比例
    return 1.0 * front_count / pointcloud.cols;
}

原理（Cheirality Constraint）：

真实姿态下，大部分三角化点应在两相机前方（深度 > 0）

错误姿态会导致大量点在后方（深度 < 0）

选择前方点比例最高的旋转

完整流程图：

**输入: corres (特征匹配点对)
↓
提取归一化坐标 (ll, rr)
↓
估计基础矩阵 F (cv::findFundamentalMat)
↓
SVD分解得到4种候选姿态:
(R1, t1), (R1, t2), (R2, t1), (R2, t2)
↓
修正符号（确保旋转矩阵行列式=+1）
↓
对4种组合分别做三角化测试:

三角化得到3D点云
统计深度>0的点数
计算前方点比例
↓
选择比例最高的旋转矩阵
↓
转换为Eigen格式并返回**

Vins-Fusion之 相机—IMU在线标定（两帧间旋转估计）（十）

步骤2：估计基础矩

步骤3：分解基础矩阵得到候选姿态

步骤4：三角化测试选择正确解

Vins-Fusion之相机—IMU在线标定（两帧间旋转估计）（十）