ORBSLAM中的三角化—理论与实现

约定不等于承诺〃 2021-12-24 05:21 317阅读 0赞

《slam十四讲》一书中有解释三角化，结论就是有了对极约束就可以三角化将地图点恢复出来。但是在ORBSLAM中三角化的代码部分，所用到的方法要更高级。

理论——

所涉及到的理论参见《Multi View Geometry in Computer Vision》第321页。这一页如下，解释的很简练，你可以再参照书中的其他内容帮助理解。

大致意思就是，得到A矩阵，解AX=0，X即3D点位置。 求解AX=0可以用SVD方法。

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80Mzc5NTM5NQ_size_16_color_FFFFFF_t_70][]

实现——

/**
     * @brief 给定投影矩阵P1,P2和图像上的点kp1,kp2，从而恢复3D坐标
     *
     * @param kp1 特征点, in reference frame
     * @param kp2 特征点, in current frame
     * @param P1  投影矩阵P1
     * @param P2  投影矩阵P２
     * @param x3D 三维点
     * @see       Multiple View Geometry in Computer Vision - 12.2 Linear triangulation methods p312
     */
    void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
    {
        // 在DecomposeE函数和ReconstructH函数中对t有归一化
        // 这里三角化过程中恢复的3D点深度取决于 t 的尺度，
        // 但是这里恢复的3D点并没有决定单目整个SLAM过程的尺度
        // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变
    
        cv::Mat A(4,4,CV_32F);
    
        A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
        A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
        A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
        A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);
    
        cv::Mat u,w,vt;
        cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
        x3D = vt.row(3).t();
        x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
    }

SVD分解： A=W\*U\*VT。 W和VT都是酉矩阵，X的解即为VT矩阵的最后一列。

具体原理这篇博客中说的较好：[https://www.cnblogs.com/yepeichu/p/10792899.html][https_www.cnblogs.com_yepeichu_p_10792899.html]

时间关系不再赘述，如有不懂的地方可以留言。

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80Mzc5NTM5NQ_size_16_color_FFFFFF_t_70]: /images/20211223/1b6d2423914947e8be2a553ea351ac72.png
[https_www.cnblogs.com_yepeichu_p_10792899.html]: https://www.cnblogs.com/yepeichu/p/10792899.html