VINS 细节系列 - Pnp （3D-2D）

最新推荐文章于 2024-07-12 22:22:12 发布

他人是一面镜子，保持谦虚的态度

最新推荐文章于 2024-07-12 22:22:12 发布

阅读量1.2k

点赞数 8

分类专栏：视觉SLAM基础理论文章标签：自动驾驶 opencv

本文链接：https://blog.csdn.net/hltt3838/article/details/109560800

版权

视觉SLAM基础理论专栏收录该内容

50 篇文章 94 订阅

订阅专栏

一、理论部分

1. DLT，直接线性变换

注意：原版的DLT中无须已知摄像机内参（请参考 MVG）
鉴于在大部分情况下，我们是已知摄像机内参的，因此这里的推导考虑了摄像机内参数。

1.1 理论推导

3D参考点在世界坐标系下的齐次坐标记为

2D投影点在图像坐标系下的齐次坐标记为

相机的内参数为

那么3D点到2D点投影可表示为

连接： https://blog.csdn.net/hltt3838/article/details/109464869

三、程序解析

void solvePnP( objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, false, flags )

objectPoints     -   世界坐标系下的特征点的坐标（注意）
imagePoints     -   在图像坐标系下对应的特征点的坐标（注意）
cameraMatrix -   相机的内参矩阵
distCoeffs          -   相机的畸变系数
rvec    -   输出的旋转向量，使坐标点从世界坐标系旋转到相机坐标系
tvec                      -   输出的平移向量，使坐标点从世界坐标系平移到相机坐标系
flags -   默认使用CV_ITERATIV迭代法

程序入口在

       if (solver_flag == INITIAL)
    {
        //stereo + IMU initilization，双目初始化
        if(STEREO && USE_IMU)
        {
            f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
            f_manager.triangulate(frame_count, Ps, Rs, tic, ric);

其中 f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric) 如下：

frame_count：当前窗口帧数；
Ps，Rs：世界坐标系下，当前 bk的位置和姿态
tic, ric：相机坐标系和bk间的转换

void FeatureManager::initFramePoseByPnP(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])
{

    if(frameCnt > 0)
    {
        vector<cv::Point2f> pts2D;
        vector<cv::Point3f> pts3D;
        
        for (auto &it_per_id : feature)        //遍历滑窗内所有的特征点
        {
            if (it_per_id.estimated_depth > 0) //满足特征点深度大于0
            {
                int index = frameCnt - it_per_id.start_frame;
                if((int)it_per_id.feature_per_frame.size() >= index + 1)
                {
                    Vector3d ptsInCam = ric[0] * (it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth) + tic[0];
 
                    Vector3d ptsInWorld = Rs[it_per_id.start_frame] * ptsInCam + Ps[it_per_id.start_frame];

                    cv::Point3f point3d(ptsInWorld.x(), ptsInWorld.y(), ptsInWorld.z());
                    cv::Point2f point2d(it_per_id.feature_per_frame[index].point.x(), it_per_id.feature_per_frame[index].point.y());
                    
                    pts3D.push_back(point3d);
                    pts2D.push_back(point2d); 
                }
            }
        }
        
        Eigen::Matrix3d RCam;
        Eigen::Vector3d PCam;
        
        RCam = Rs[frameCnt - 1] * ric[0];
        PCam = Rs[frameCnt - 1] * tic[0] + Ps[frameCnt - 1];

        if(solvePoseByPnP(RCam, PCam, pts2D, pts3D))
        {
            // Rs[frameCnt] = Rw_c * Rb_c.transpose() = Rw_b
            Rs[frameCnt] =  RCam * ric[0].transpose(); 
            Ps[frameCnt] = -RCam * ric[0].transpose() * tic[0] + PCam;

            //对应的四元素
            Eigen::Quaterniond Q(Rs[frameCnt]);
        }
    }
}

对上面的程序进行一步步解析：

第一部分：

void FeatureManager::initFramePoseByPnP(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])
{

    if(frameCnt > 0)
    {
        vector<cv::Point2f> pts2D;
        vector<cv::Point3f> pts3D;
        
        for (auto &it_per_id : feature)        //遍历滑窗内所有的特征点
        {
            if (it_per_id.estimated_depth > 0) //满足特征点深度大于0
            {
                int index = frameCnt - it_per_id.start_frame;
                if((int)it_per_id.feature_per_frame.size() >= index + 1)
                {
                    Vector3d ptsInCam = ric[0] * (it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth) + tic[0];
                    Vector3d ptsInWorld = Rs[it_per_id.start_frame] * ptsInCam + Ps[it_per_id.start_frame];

                    cv::Point3f point3d(ptsInWorld.x(), ptsInWorld.y(), ptsInWorld.z());
                    cv::Point2f point2d(it_per_id.feature_per_frame[index].point.x(), it_per_id.feature_per_frame[index].point.y());
                    
                    pts3D.push_back(point3d);
                    pts2D.push_back(point2d); 
                }
            }

该程序只有在滑动窗口中有图像才进入，对应 if(frameCnt > 0) ，废话！首先定义了两个变量 pts2D, pts3D; 分别用于存放世界坐标系下的3D点和其对应的相机归一化坐标系下的2D点；整个过程就是图像帧一帧一帧的进来，会对周围特征点进行观测, 假如 滑动窗口大小为 10，当前进来了5帧；VINS的策略是当滑动窗口不满时，一直进来图像，提取特征点，三角化，这些帧都是看作关键帧，直到满足滑窗帧数；

变量解释：

frameCnt：当前滑动窗口中的帧数；

it_per_id.start_frame：特征点被第一次观测到时，对应滑动窗口的帧号

feature_per_frame.size() ：表示观测到某个路标点图像的个数

feature_per_frame[0].point： 某个路标点被第一张图像观测到的信息，所谓的第一张图片不一定是滑窗内的第一张

ric[0]：相机参考坐标系的点到对应的bk坐标系上点的转换矩阵- Rbk_ck, 这个值在在线调节外参的时候就定了，不再变了！

Rb0_c0 = Rb1_c1 = Rb2_c2 ... Rbn_cn

tic[0]: 是两者的平移

Ps、Rs、Vs： bk相对世界坐标系w 的位置、姿态和速度； k代表的是与bk对应的帧，记住啦！

这个图是我自己想的，也有可能不对！

记住啦，图像是一帧一帧进来的！我们以第五帧（C5）为讲解，假设对于特征点P5

it_per_id.feature_per_frame.size() = 3 满足下面的条件

if((int)it_per_id.feature_per_frame.size() >= index + 1)

仔细想一下，为什么会有这个判断，再看看其他特征点，都会满足这个条件，那不是多此一举嘛？

但是我们的滑动窗口大小是10，而当前帧才是5，还会进入图像，都满足上面的判断，但是当滑动窗口满时，我们进来新的帧需要边缘化，上面的条件可能就不满足了，你们可以考虑一下！比如最新帧被边缘化调，那这个时候就没有必要求最新帧的PnP了，这个应该不难理解！

注意啦！感觉这样处理的原因是因为 Opencv中 cv::solvePnP 这个函数，C5进来的时候我们观测到了P5，3D点便是前面帧获得的P5 世界坐标系的点，相对C3而言； 2D点却不是C3中的坐标，而是C5观察到 P5 下的归一化坐标！然后根据已知的C3到世界坐标系w的转换矩阵，带入 Opencv中 cv::solvePnP 中，得到 C5到w的转换矩阵。

第二部分：

        Eigen::Matrix3d RCam;
        Eigen::Vector3d PCam;

        RCam = Rs[frameCnt - 1] * ric[0];
        PCam = Rs[frameCnt - 1] * tic[0] + Ps[frameCnt - 1];

因为滑动窗口进来帧嘛，知道倒数第二帧 frameCnt - 1 相机坐标系的点到世界坐标系点的转换矩阵；

RCam = Rs[frameCnt - 1] * ric[0] // RCam = Rw_bk-1 * Rbk-1_ck-1 = Rw_ck-1
想求新进来帧的 Rs[frameCnt]、Ps[frameCnt]

第三部分：


        if(solvePoseByPnP(RCam, PCam, pts2D, pts3D))
        {
            // Rs[frameCnt] = Rw_c * Rb_c.transpose() = Rw_b
            Rs[frameCnt] =  RCam * ric[0].transpose(); 
            Ps[frameCnt] = -RCam * ric[0].transpose() * tic[0] + PCam;

            //对应的四元素
            Eigen::Quaterniond Q(Rs[frameCnt]);
        }
    }
}

solvePoseByPnP(RCam, PCam, pts2D, pts3D) 如下：

bool FeatureManager::solvePoseByPnP(Eigen::Matrix3d &R, Eigen::Vector3d &P, 
                                      vector<cv::Point2f> &pts2D, vector<cv::Point3f> &pts3D)
{
    Eigen::Matrix3d R_initial;
    Eigen::Vector3d P_initial;

    //坐标系之间的转换：w_T_cam ---> cam_T_w，注意和点之间转换的区别
    R_initial = R.inverse();
    P_initial = -(R_initial * P);

   
    if (int(pts2D.size()) < 4)
    {
        printf("feature tracking not enough, please slowly move you device! \n");
        return false;
    }
    cv::Mat r, rvec, t, D, tmp_r;
    cv::eigen2cv(R_initial, tmp_r);//cv::Mat 与 Eigen 转换
    cv::Rodrigues(tmp_r, rvec);    //罗德里格斯公式
    cv::eigen2cv(P_initial, t);     //cv::Mat 与 Eigen 转换
    
    cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);  
    bool pnp_succ;
    pnp_succ = cv::solvePnP(pts3D, pts2D, K, D, rvec, t, 1);//用的是相似三角形方法！
   

    if(!pnp_succ)
    {
        printf("pnp failed ! \n");
        return false;
    }
    
    cv::Rodrigues(rvec, r);
    
     
    Eigen::MatrixXd R_pnp;
    cv::cv2eigen(r, R_pnp);
    Eigen::MatrixXd T_pnp;
    cv::cv2eigen(t, T_pnp);

    // cam_T_w ---> w_T_cam
    R = R_pnp.transpose();
    P = R * (-T_pnp);

    return true;
}