3D-2D:PnP_DLT.P3P

最新推荐文章于 2024-11-15 12:26:52 发布

古路

最新推荐文章于 2024-11-15 12:26:52 发布

阅读量1.1k

点赞数

分类专栏： slam # 十四讲文章标签： 3D-2D：PnP

本文链接：https://blog.csdn.net/fb_941219/article/details/89785493

版权

slam 同时被 2 个专栏收录

70 篇文章 24 订阅

订阅专栏

十四讲

17 篇文章 2 订阅

订阅专栏

3D-2D:PnP

引言

ch7.
PnP(Perspective-n-Point)是求解3D到2D点对运动的方法.已知 $n$ 个空间点 $[X, Y, Z, 1]$ (前一帧的齐次坐标系)以及后一帧对应特征点坐标 $u_1,v_1,1)$ (归一化平面坐标系)。
在这里插入图片描述
$\left[ \begin{matrix} u_1 \\ v_1 \\ 1 \end{matrix} \right] = [R|t]_{3*4} \left[ \begin{matrix} X \\ Y \\ Z \\ 1 \end{matrix} \right]_{4*1}$

1.直接线性变换(DLT)

在这里插入图片描述

在这里插入图片描述
以图像理解，不然以坐标系转换来看公式容易理解错，而且推不通！在DLT求解中，直接将T看成了12个未知数，忽略了他们之间的联系。因为旋转矩阵 $R\in SO(3)$ ,用DLT求解出的矩阵不一定满足这个约束，它只是个一般矩阵。平移向量好办，它属于空间向量。对与旋转矩阵 $R$ ，必须针对DLT估计 $T$ 的左边 $3 * 3$ 的矩阵块，寻找一个最好的旋转矩阵对它进行近似。这可以由 $Q R$ 分解完成，相当于把结果从矩阵空间重新投影到 $S E (3)$ 流形上,转换成旋转和平移两部分。

CODE:

    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );// 相机内参,TUM Freiburg2
    vector<Point3f> pts_3d;//3D点  第一幅图像中的 特征点 对应的 3维点
    vector<Point2f> pts_2d;//2D点  第二幅图像中的 特征点
    for ( DMatch m:matches )
    {
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];//匹配点对 对应的深度
        if ( d == 0 )   // bad depth
            continue;
        float dd = d/1000.0;//深度单位为 毫米 mm  转换为m  除去尺度因子
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );// 像素坐标转相机归一化坐标  x，y，1
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );// 3D点  第一幅图像中的 特征点 对应的 3维点
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );// 2D点  第二幅图像中的 特征点
    }

    cout<<"3d-2d 点对数 : "<<pts_3d.size() <<endl;
    // 利用 PnP 求解初始解
    //只利用3个 3D - 2D 点对
    Mat r, t;//得到 初始 旋转向量r 和  平移矩阵t
    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    Mat R;//旋转矩阵
    cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵 罗德里的公式 旋转向量 得到 旋转矩阵
    cout<<"初始旋转矩阵 R="<<endl<<R<<endl;
    cout<<"初始平移向量 t="<<endl<<t<<endl;

2.P3P

略