视觉里程计基础学习:对极约束求解估计相机运动、显示出筛选特征点后的图像

本文介绍了视觉里程计的基础知识,包括对极约束原理及其在求解相机运动中的应用。通过匹配特征点,利用对极约束公式可以估算相机的旋转和平移。文中还展示了筛选特征点后的图像,并提供了完整的代码实现。
摘要由CSDN通过智能技术生成

视觉里程计基础学习:对极约束求解估计相机运动显示出筛选特征点后的图像

对极约束原理

求取两帧图像 I1, I2 之间的运动,设第一帧到第二帧的运动为R, t。两个相机中心分别为 O1, O2。现在,考虑 I1 中有一个特征点 p1,它在 I2 中对应着特征点 p2。我们晓得这俩是通过特征匹配得到的。如果匹配正确,说明它们确实是同一个空间点在两个成像平面上的投影。这里我们需要一些术语来描述它们之间的几何关系。首先,连线 −−−→O1p1 和连线 −−−→O2p2 在三维空间中会相交于点 P。这时候点 O1, O2, P 三个点可以确定一个平面,称为极平面(Epipolar plane)。O1O2 连线与像平面 I1, I2 的交点分别为 e1, e2。e1, e2,称为极(Epipoles),O1O2 被称为基线(Baseline)。称极平面与
两个像平面 I1, I2 之间的相交线 l1, l2 为极线(Epipolar line)。
在这里插入图片描述
直观上讲,从第一帧的角度上看,射线 −−−→O1p1 是某个像素可能出现的空间位置——因
为该射线上的所有点都会投影到同一个像素点。同时,如果不知道 P 的位置,那么当我们
在第二个图像上看时,连线 −−→e2p2(也就是第二个图像中的极线)就是 P 可能出现的投影的位置,也就是射线 −−−→O1p1 在第二个相机中的投影。现在,由于我们通过特征点匹配,确定了 p2 的像素位置,所以能够推断 P 的空间位置,以及相机的运动。
在第一帧的坐标系下,设 P 的空间位置为:P = [X, Y, Z]T .
根据针孔相机模型
在这里插入图片描述

我们知道两个像素点 p1, p2 的像素位置为:s1p1 = KP , s2p2 = K (RP + t)
这里 K 为相机内参矩阵,R, t 为两个坐标系的相机运动如果使用齐次坐标,我们也可以把上式写成在乘以非零常数下成立的等式:p1 = KP , p2 = K (RP + t)
取x1 = K 1p1, x2 = K 1p2,这里的 x1, x2 是两个像素点的归一化平面上的坐标
代入上式得:x2 = Rx1 + t
两边同时与t做外积,然后在左乘xT得:xT2 t∧x2 = xT2 t∧Rx1
在这里插入图片描述
这两个式子都称为对极约束。它的几何意义是 O1, P, O2 三者共面。对极约束中同时包含了平移和旋转。
把中间部分记作两个矩阵:基础矩阵(FundamentalMatrix)F 和本质矩阵(Essential Matrix)E,可以进一步简化对极约束:
在这里插入图片描述
对极约束简洁地给出了两个匹配点的空间位置关系
相机位姿估计问题就分为了两步:

  1. 根据配对点的像素位置,求出 E 或者 F;
  2. 根据 E 或者 F,求出 R, t。
    E为本质矩阵,F为基础矩阵,
    后面还会有单应矩阵H,它描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上(墙面、地面等),则可以通过单应性来进行运动估计。

对极约束求解估计相机运动

//-- 估计两张图像间运动
    Mat R,t;
    pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t );

    //-- 验证E=t^R*scale
    //求t的反对称矩阵
    Mat t_x = ( Mat_<double> ( 3,3 ) << 0,-t.at<double> ( 2,0 ),     t.at<double> ( 1,0 ),
                t.at<double> ( 2,0 ),      0,                      -t.at<double> ( 0,0 ),
                -t.at<double> ( 1.0 ),     t.at<double> ( 0,0 ),      0 );

    cout<<"t^R="<<endl<<t_x*R<<endl;

    //-- 验证对极约束
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    for ( DMatch m: matches )
    {
        Point2d pt1 = pixel2cam ( keypoints_1[ m.queryIdx ].pt, K );
        Mat y1 = ( Mat_<double> ( 3,1 ) << pt1.x, pt1.y, 1 );
        Point2d pt2 = pixel2cam ( keypoints_2[ m.trainIdx ].pt, K );
        Mat y2 = ( Mat_<double> ( 3,1 ) << pt2.x, pt2.y, 1 );
        Mat d = y2.t() * t_x * R * y1;
        cout << "epipolar constraint = " << d << endl;
    }
    return 0;
}

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3 
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2 
    // Ptr<FeatureDetector> detector = FeatureDetec
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值