《视觉SLAM十四讲》slambook/ch7/gpose_estimation_2d2d/详细版注释

 

#include 
#include 
#include 
#include 
#include 
// #include "extra.h" // use this if in OpenCV2 
using namespace std;
using namespace cv;

/****************************************************
 * 本程序演示了如何使用2D-2D的特征匹配估计相机运动
 * **************************************************/

void find_feature_matches (
    const Mat& img_1, const Mat& img_2,
    std::vector& keypoints_1,
    std::vector& keypoints_2,
    std::vector< DMatch >& matches );

void pose_estimation_2d2d (
    std::vector keypoints_1,
    std::vector keypoints_2,
    std::vector< DMatch > matches,
    Mat& R, Mat& t );

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

int main ( int argc, char** argv )
{
    if ( argc != 3 )
    {
        cout<<"usage: pose_estimation_2d2d img1 img2"< keypoints_1, keypoints_2;
    vector matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"< ( 3,3 ) <<
                0,                      -t.at ( 2,0 ),     t.at ( 1,0 ),
                t.at ( 2,0 ),      0,                      -t.at ( 0,0 ),
                -t.at ( 1.0 ),     t.at ( 0,0 ),      0 );

    cout<<"t^R="< ( 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_ ( 3,1 ) << pt1.x, pt1.y, 1 );
        Point2d pt2 = pixel2cam ( keypoints_2[ m.trainIdx ].pt, K );
        Mat y2 = ( Mat_ ( 3,1 ) << pt2.x, pt2.y, 1 );
        Mat d = y2.t() * t_x * R * y1; //理论值应该为0，所以可以验证d是否为0来验证
        cout << "epipolar constraint = " << d << endl;
    }
    return 0;
}

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector& keypoints_1,
                            std::vector& keypoints_2,
                            std::vector< DMatch >& matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3 
    Ptr detector = ORB::create();
    Ptr descriptor = ORB::create();
    // use this if you are in OpenCV2 
    // Ptr detector = FeatureDetector::create ( "ORB" );
    // Ptr descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector match;
    //BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, match );

    //-- 第四步:匹配点对筛选
    double min_dist=10000, max_dist=0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = match[i].distance;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( match[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            matches.push_back ( match[i] );
        }
    }
}


Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at ( 0,2 ) ) / K.at ( 0,0 ),
               ( p.y - K.at ( 1,2 ) ) / K.at ( 1,1 )
           );
}


void pose_estimation_2d2d ( std::vector keypoints_1,
                            std::vector keypoints_2,
                            std::vector< DMatch > matches,
                            Mat& R, Mat& t ) 
{
    // 相机内参,TUM Freiburg2
    Mat K = ( Mat_ ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );

    //-- 把匹配点转换为vector的形式
    vector points1;
    vector points2;

    for ( int i = 0; i < ( int ) matches.size(); i++ )
    {
        points1.push_back ( keypoints_1[matches[i].queryIdx].pt );
        points2.push_back ( keypoints_2[matches[i].trainIdx].pt );
    }

    //-- 计算基础矩阵
    Mat fundamental_matrix;
    fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT );
    cout<<"fundamental_matrix is "<8个点，求解超定线性方程组————最小二乘
        2、矩阵分解：
            方法：SVD
            注意：排除错误解、尺度不确定性导致t的归一化
    2、H方法：适用于平面的情况，且H中包含了平面的信息。
        1、求解H：
            1、解析法：=4点法，求解正定线性方程组————4点法
            2、估计法：>4个点，求解超定线性方程组————最小二乘
        2、矩阵分解：
            方法：数值法、解析法（参考ORB_SLAM、SVO）
            注意：排除错误解、尺度不确定性导致t的归一化
2、opencv：
    1、无H分解API
    2、t是默认归一化
    3、E分解、F分解时默认排除错误解
3、注意事项：
    1、尺度不确定性
    2、初始化纯旋转
    3、特征点共面
    4、多于8个点
*/

初学者，恳请指正，欢迎讨论！