今夜喜雨 | 直线线性三角化理论

阳光穿透心脏的1/2处 2021-12-12 23:43 263阅读 0赞

雨夜，用了两个小时的时间终于推导了直线三角化的公式。直线三角化？

是的，即通过两帧的匹配直线得到空间直线的端点，如果端点严格匹配那么就是点的三角化，你可以从《SLAM十四讲》一书或者其他的博客很容易知道怎么做。但是直线的端点往往不是严格对齐的，那么怎么办？ 也不难办，只要你能明白下面两个铺垫。所涉基本理论这里我直接引用。

关于这个内容，在多视图几何一书中暂时没有找到，因此参考点的三角化理论（参见我的另一篇博客），本文对直线的线性三角化方法做了阐述。

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

### 第一 **特征点三角化**  ###

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80Mzc5NTM5NQ_size_16_color_FFFFFF_t_70][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80Mzc5NTM5NQ_size_16_color_FFFFFF_t_70 1][]

来自： [https://www.cnblogs.com/yepeichu/p/10792899.html][https_www.cnblogs.com_yepeichu_p_10792899.html]

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

### 第二 直线参数化初认识之普吕克参数化 ###

这一部分关系不大，学习一下就好。

作者：robot  
链接：https://zhuanlan.zhihu.com/p/65674067  
来源：知乎

![aHR0cHM6Ly9waWM0LnpoaW1nLmNvbS92Mi0zMWEzMDkxY2VlMzE5NTgxNDUyZTUzMDZlOTg1NjZmYl9iLmpwZw][]

图1 线特征的Plücker坐标 （a）Plücker线坐标系 （b）新观测到的线的初始化

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

### 第三 直线的线性三角化推导！ ###

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80Mzc5NTM5NQ_size_16_color_FFFFFF_t_70 2][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80Mzc5NTM5NQ_size_16_color_FFFFFF_t_70 3][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80Mzc5NTM5NQ_size_16_color_FFFFFF_t_70 4][]

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

### 第四 如何写代码 ###

/**
     * @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);
    }

照着上面改就行。

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

云山苍苍，江水泱泱。 2019.7.12 午夜

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80Mzc5NTM5NQ_size_16_color_FFFFFF_t_70]: /images/20211210/5dfed1e7e61a4a519fe218a481d9c860.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80Mzc5NTM5NQ_size_16_color_FFFFFF_t_70 1]: /images/20211210/e5594a1863ca4408b1b2c1699ef4adec.png
[https_www.cnblogs.com_yepeichu_p_10792899.html]: https://www.cnblogs.com/yepeichu/p/10792899.html
[aHR0cHM6Ly9waWM0LnpoaW1nLmNvbS92Mi0zMWEzMDkxY2VlMzE5NTgxNDUyZTUzMDZlOTg1NjZmYl9iLmpwZw]: /images/20211210/373653231f1f4707a97cad1d85953053.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80Mzc5NTM5NQ_size_16_color_FFFFFF_t_70 2]: /images/20211210/867666faaea349ddbb839741de53c420.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80Mzc5NTM5NQ_size_16_color_FFFFFF_t_70 3]: /images/20211210/953f9d06db57483b9b24c96fc385eb22.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80Mzc5NTM5NQ_size_16_color_FFFFFF_t_70 4]: /images/20211210/6f14128047e1473b9ddc66f828cde8d7.png