SLAM源码分析（五）

最新推荐文章于 2023-02-15 22:06:44 发布

wang_yq0728

最新推荐文章于 2023-02-15 22:06:44 发布

阅读量121

点赞数

文章标签： slam

本文链接：https://blog.csdn.net/wang_yq0728/article/details/121209508

版权

本文详细介绍了两视图重建过程中如何通过基础矩阵和单应性矩阵恢复旋转和平移参数。具体包括从基础矩阵计算本质矩阵，分解本质矩阵得到四对运动假设，以及通过检查每对假设的旋转和平移重建3D点的数量和视差来选择最佳解。此外，还阐述了单应性矩阵的Faugeras分解方法，计算旋转和平移，以及通过比较不同解的质量来确定最佳的两视图重建结果。

摘要由CSDN通过智能技术生成

2021SC@SDUSC

TwoViewReconstruction.cc

函数ReconstrustF，通过基础矩阵重建

K 相机内参

R21 旋转（要输出的）

t21 平移（要输出的）

vP3D 恢复的三维点（要输出的）

vbTriangulated 大小与mvKeys1一致，表示哪个点被重建了

minParallax 1

minTriangulated 50

bool TwoViewReconstruction::ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,
                                         cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D,
                                         vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{
    // 统计了合法的匹配，后面用于对比重建出的点数
    int N = 0;
    for (size_t i = 0, iend = vbMatchesInliers.size(); i < iend; i++)
        if (vbMatchesInliers[i])
            N++;

    // Compute Essential Matrix from Fundamental Matrix
    // 1. 计算本质矩阵
    cv::Mat E21 = K.t() * F21 * K;

    cv::Mat R1, R2, t;

    // Recover the 4 motion hypotheses
    // 2. 分解本质矩阵，得到4对rt
    DecomposeE(E21, R1, R2, t);

    cv::Mat t1 = t;
    cv::Mat t2 = -t;

    // Reconstruct with the 4 hyphoteses and check
    // 3. 使用四对结果分别重建
    vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;
    vector<bool> vbTriangulated1, vbTriangulated2, vbTriangulated3, vbTriangulated4;
    float parallax1, parallax2, parallax3, parallax4;

    int nGood1 = CheckRT(R1, t1, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D1, 4.0 * mSigma2, vbTriangulated1, parallax1);
    int nGood2 = CheckRT(R2, t1, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D2, 4.0 * mSigma2, vbTriangulated2, parallax2);
    int nGood3 = CheckRT(R1, t2, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D3, 4.0 * mSigma2, vbTriangulated3, parallax3);
    int nGood4 = CheckRT(R2, t2, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D4, 4.0 * mSigma2, vbTriangulated4, parallax4);
    // 统计重建出点的数量最大值
    int maxGood = max(nGood1, max(nGood2, max(nGood3, nGood4)));

    R21 = cv::Mat();
    t21 = cv::Mat();
    // 起码要重建出超过百分之90的匹配点
    int nMinGood = max(static_cast<int>(0.9 * N), minTriangulated);

    // 4. 看看有没有脱颖而出的结果，且最大值要高于要求的最低值，如果大家都差不多说明有问题，因为4个结果中只有一个会正常
    // 大家都很多的话反而不正常了。。。
    int nsimilar = 0;
    if (nGood1 > 0.7 * maxGood)
        nsimilar++;
    if (nGood2 > 0.7 * maxGood)
        nsimilar++;
    if (nGood3 > 0.7 * maxGood)
        nsimilar++;
    if (nGood4 > 0.7 * maxGood)
        nsimilar++;

    // If there is not a clear winner or not enough triangulated points reject initialization
    if (maxGood < nMinGood || nsimilar > 1)
    {
        return false;
    }

    // If best reconstruction has enough parallax initialize
    // 5. 使用最好的结果输出
    if (maxGood == nGood1)
    {
        if (parallax1 > minParallax)
        {
            vP3D = vP3D1;
            vbTriangulated = vbTriangulated1;

            R1.copyTo(R21);
            t1.copyTo(t21);
            return true;
        }
    }
    else if (maxGood == nGood2)
    {
        if (parallax2 > minParallax)
        {
            vP3D = vP3D2;
            vbTriangulated = vbTriangulated2;

            R2.copyTo(R21);
            t1.copyTo(t21);
            return true;
        }
    }
    else if (maxGood == nGood3)
    {
        if (parallax3 > minParallax)
        {
            vP3D = vP3D3;
            vbTriangulated = vbTriangulated3;

            R1.copyTo(R21);
            t2.copyTo(t21);
            return true;
        }
    }
    else if (maxGood == nGood4)
    {
        if (parallax4 > minParallax)
        {
            vP3D = vP3D4;
            vbTriangulated = vbTriangulated4;

            R2.copyTo(R21);
            t2.copyTo(t21);
            return true;
        }
    }

    return false;
}

函数ReconstructH,

从H恢复R t。H矩阵分解常见有两种方法：Faugeras SVD-based decomposition 和 Zhang SVD-based decomposition

参考文献：Motion and structure from motion in a piecewise plannar environment

- Faugeras et al, Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988.

- Deeper understanding of the homography decomposition for vision-based control

设平面法向量 n = (a, b, c)^t 有一点(x, y, z)在平面上，则ax + by + cz = d 即 1/d * n^t * x = 1 其中d表示

x' = R*x + t 从下面开始x 与 x'都表示归一化坐标

λ2*x' = R*(λ1*x) + t = R*(λ1*x) + t * 1/d * n^t * (λ1*x)

x' = λ*(R + t * 1/d * n^t) * x = H^ * x

对应图像坐标 u' = G * u G = KH^K.inv

H^ ~= d*R + t * n^t = U∧V^t ∧ = U^t * H^ * V = d*U^t * R * V + (U^t * t) * (V^t * n)^t

s = det(U) * det(V) s 有可能是 1 或 -1 ∧ = s^2 * d*U^t * R * V + (U^t * t) * (V^t * n)^t = (s*d) * s * U^t * R * V + (U^t * t) * (V^t * n)^t

令 R' = s * U^t * R * V t' = U^t * t n' = V^t * n d' = s * d

∧ = d' * R' + t' * n'^t 所以∧也可以认为是一个单应矩阵，其中加入s只为说明有符号相反的可能

设∧ = | d1, 0, 0 | 取e1 = (1, 0, 0)^t e2 = (0, 1, 0)^t e3 = (0, 0, 1)^t

| 0, d2, 0 |

| 0, 0, d3 |

n' = (a1, 0, 0)^t + (0, b1, 0)^t + (0, 0, c1)^t = a1*e1 + b1*e2 + c1*e3

∧ = [d1*e1, d2*e2, d3*e3] = [d' * R' * e1, d' * R' * e2, d' * R' * e3] + [t' * a1, t' * b1, t' * c1]

==> d1*e1 = d' * R' * e1 + t' * a1

d2*e2 = d' * R' * e2 + t' * b1

d3*e3 = d' * R' * e3 + t' * c1

上面式子每两个消去t可得

d'R'(b1e1 - a1e2) = d1b1e1 - d2a1e2

d'R'(c1e2 - b1e3) = d2c1e1 - d3b1e3 同时取二范数，因为旋转对二范数没影响，所以可以约去

d'R'(a1e3 - c1e1) = d3a1e3 - d1c1e1

(d'^2 - d2^2)*a1^2 + (d'^2 - d1^2)*b1^2 = 0

(d'^2 - d3^2)*b1^2 + (d'^2 - d2^2)*c1^2 = 0 令 d'^2 - d1^2 = x1 d'^2 - d2^2 = x2 d'^2 - d3^2 = x3

(d'^2 - d1^2)*c1^2 + (d'^2 - d3^2)*a1^2 = 0

| x2 x1 0 | | a1^2 | | 0 x3 x2 | * | b1^2 | = 0 ===> x1 * x2 * x3 = 0 '^2 - d1^2) * (d'^2 - d2^2) * (d'^2 - d3^2) = 0 | x3 0 x1 | | c1^2 |

由于d1 >= d2 >= d3 所以d' = d2 or -d2

--------------------------------------------------------------------------------------------------------------------------------

下面分情况讨论，当d' > 0 再根据a1^2 + b1^2 + c1^2 = 1 ??????哪来的根据，不晓得

能够求得a1 = ε1 * sqrt((d1^2 - d2^2) / (d1^2 - d3^2))

b1 = 0

c1 = ε2 * sqrt((d2^2 - d3^2) / (d1^2 - d3^2)) 其中ε1 ε2 可以为正负1

结果带入 d2*e2 = d' * R' * e2 + t' * b1 => R' * e2 = e2

| cosθ 0 -sinθ |

==> R' = | 0 1 0 | n' 与 R' 带入 d'R'(a1e3 - c1e1) = d3a1e3 - d1c1e1

| sinθ 0 cosθ |

| cosθ 0 -sinθ | | -c1 | | -d1c1 |

d' * | 0 1 0 | * | 0 | = | 0 | 能够解出 sinθ 与 cosθ

| sinθ 0 cosθ | | a1 | | d3a1 |

到此为止得到了 R' 再根据 d1*e1 = d' * R' * e1 + t' * a1

d2*e2 = d' * R' * e2 + t' * b1

d3*e3 = d' * R' * e3 + t' * c1

求得 t' = (d1 - d3) * (a1, 0, c1)^t

bool TwoViewReconstruction::ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
                                         cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D,
                                         vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{
    // 统计了合法的匹配，后面用于对比重建出的点数
    int N = 0;
    for (size_t i = 0, iend = vbMatchesInliers.size(); i < iend; i++)
        if (vbMatchesInliers[i])
            N++;

    // We recover 8 motion hypotheses using the method of Faugeras et al.
    // Motion and structure from motion in a piecewise planar environment.
    // International Journal of Pattern Recognition and Artificial Intelligence, 1988
    // step1：SVD分解Homography
    // 因为特征点是图像坐标系，所以讲H矩阵由相机坐标系换算到图像坐标系
    cv::Mat invK = K.inv();
    cv::Mat A = invK * H21 * K;

    cv::Mat U, w, Vt, V;
    cv::SVD::compute(A, w, U, Vt, cv::SVD::FULL_UV);
    V = Vt.t();

    float s = cv::determinant(U) * cv::determinant(Vt);

    float d1 = w.at<float>(0);
    float d2 = w.at<float>(1);
    float d3 = w.at<float>(2);
    // SVD分解的正常情况是特征值降序排列
    if (d1 / d2 < 1.00001 || d2 / d3 < 1.00001)
    {
        return false;
    }

    vector<cv::Mat> vR, vt, vn;
    vR.reserve(8);
    vt.reserve(8);
    vn.reserve(8);

    // n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1
    // step2：计算法向量
    // n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1
    // 法向量n'= [x1 0 x3]
    float aux1 = sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3));
    float aux3 = sqrt((d2 * d2 - d3 * d3) / (d1 * d1 - d3 * d3));
    float x1[] = {aux1, aux1, -aux1, -aux1};
    float x3[] = {aux3, -aux3, aux3, -aux3};

    // case d'=d2
    // step3：恢复旋转矩阵
    // step3.1：计算 sin(theta)和cos(theta)，case d'=d2
    float aux_stheta = sqrt((d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) / ((d1 + d3) * d2);

    float ctheta = (d2 * d2 + d1 * d3) / ((d1 + d3) * d2);
    float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};

    // step3.2：计算四种旋转矩阵R，t
    // 计算旋转矩阵 R‘，计算ppt中公式18
    //      | ctheta      0   -aux_stheta|       | aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      | aux_stheta  0    ctheta    |       |-aux3|

    //      | ctheta      0    aux_stheta|       | aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      |-aux_stheta  0    ctheta    |       | aux3|

    //      | ctheta      0    aux_stheta|       |-aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      |-aux_stheta  0    ctheta    |       |-aux3|

    //      | ctheta      0   -aux_stheta|       |-aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      | aux_stheta  0    ctheta    |       | aux3|
    for (int i = 0; i < 4; i++)
    {
        cv::Mat Rp = cv::Mat::eye(3, 3, CV_32F);
        Rp.at<float>(0, 0) = ctheta;
        Rp.at<float>(0, 2) = -stheta[i];
        Rp.at<float>(2, 0) = stheta[i];
        Rp.at<float>(2, 2) = ctheta;

        cv::Mat R = s * U * Rp * Vt;
        vR.push_back(R);

        cv::Mat tp(3, 1, CV_32F);
        tp.at<float>(0) = x1[i];
        tp.at<float>(1) = 0;
        tp.at<float>(2) = -x3[i];
        tp *= d1 - d3;

        // 这里虽然对t有归一化，并没有决定单目整个SLAM过程的尺度
        // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变
        cv::Mat t = U * tp;
        vt.push_back(t / cv::norm(t));

        cv::Mat np(3, 1, CV_32F);
        np.at<float>(0) = x1[i];
        np.at<float>(1) = 0;
        np.at<float>(2) = x3[i];

        cv::Mat n = V * np;
        if (n.at<float>(2) < 0)
            n = -n;
        vn.push_back(n);
    }

    //case d'=-d2
    // step3.3：计算 sin(theta)和cos(theta)，case d'=-d2
    float aux_sphi = sqrt((d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) / ((d1 - d3) * d2);

    float cphi = (d1 * d3 - d2 * d2) / ((d1 - d3) * d2);
    float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};

    // step3.4：计算四种旋转矩阵R，t
    // 计算旋转矩阵 R‘
    for (int i = 0; i < 4; i++)
    {
        cv::Mat Rp = cv::Mat::eye(3, 3, CV_32F);
        Rp.at<float>(0, 0) = cphi;
        Rp.at<float>(0, 2) = sphi[i];
        Rp.at<float>(1, 1) = -1;
        Rp.at<float>(2, 0) = sphi[i];
        Rp.at<float>(2, 2) = -cphi;

        cv::Mat R = s * U * Rp * Vt;
        vR.push_back(R);

        cv::Mat tp(3, 1, CV_32F);
        tp.at<float>(0) = x1[i];
        tp.at<float>(1) = 0;
        tp.at<float>(2) = x3[i];
        tp *= d1 + d3;

        cv::Mat t = U * tp;
        vt.push_back(t / cv::norm(t));

        cv::Mat np(3, 1, CV_32F);
        np.at<float>(0) = x1[i];
        np.at<float>(1) = 0;
        np.at<float>(2) = x3[i];

        cv::Mat n = V * np;
        if (n.at<float>(2) < 0)
            n = -n;
        vn.push_back(n);
    }

    int bestGood = 0;
    int secondBestGood = 0;
    int bestSolutionIdx = -1;
    float bestParallax = -1;
    vector<cv::Point3f> bestP3D;
    vector<bool> bestTriangulated;

    // Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax)
    // We reconstruct all hypotheses and check in terms of triangulated points and parallax
    // step4：d'=d2和d'=-d2分别对应8组(R t)，通过恢复3D点并判断是否在相机正前方的方法来确定最优解
    for (size_t i = 0; i < 8; i++)
    {
        float parallaxi;
        vector<cv::Point3f> vP3Di;
        vector<bool> vbTriangulatedi;
        int nGood = CheckRT(vR[i], vt[i], mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3Di, 4.0 * mSigma2, vbTriangulatedi, parallaxi);
        
        // 保留最优的和次优的
        if (nGood > bestGood)
        {
            secondBestGood = bestGood;
            bestGood = nGood;
            bestSolutionIdx = i;
            bestParallax = parallaxi;
            bestP3D = vP3Di;
            bestTriangulated = vbTriangulatedi;
        }
        else if (nGood > secondBestGood)
        {
            secondBestGood = nGood;
        }
    }

    // step5：通过判断最优是否明显好于次优，从而判断该次Homography分解是否成功
    if (secondBestGood < 0.75 * bestGood && bestParallax >= minParallax && bestGood > minTriangulated && bestGood > 0.9 * N)
    {
        vR[bestSolutionIdx].copyTo(R21);
        vt[bestSolutionIdx].copyTo(t21);
        vP3D = bestP3D;
        vbTriangulated = bestTriangulated;

        return true;
    }

    return false;
}

wang_yq0728

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
SLAM源码分析（五）

2021SC@SDUSCTwoViewReconstruction.cc函数ReconstrustF，通过基础矩阵重建K 相机内参R21 旋转（要输出的）t21 平移（要输出的）vP3D 恢复的三维点（要输出的）vbTriangulated 大小与mvKeys1一致，表示哪个点被重建了minParallax 1minTriangulated 50bool TwoViewReconstruction::ReconstructF(vector<bool>..
复制链接

扫一扫