SlamBook14讲-PoseEstimation2d2d

参考高翔老师的示例代码

在FeatureExtraction的基础上计算本质矩阵E 并计算对极约束。

FeatureExtraction部分：

SlamBook14讲-FeatureExtraction_就差一点点_的博客-CSDN博客

本质矩阵E 包含了相机运动的旋转矩阵和平移矩阵。

本质矩阵E 的求解需要匹配好的n对点和内参，这里的points类型是InputArray。后者是opencv中一种输入的数组（不可更改 const），有很多重载版本，可以用vector储存。

cv::findEssentialMat(points1,points2,K);

基础矩阵是

基础矩阵的求解不需要相机内参K 其中FM_8POINT代表8点法求解

cv::findFundamentalMat(points1,points2,cv::FM_8POINT);

求解出本质矩阵E 后 便可计算出R 和T

cv::recoverPose(EssentialMat,points1,points2,K,R,t);

可以通过验证对极约束是否接近0来判断求解是否正确，即

其中p1和p2是P分别在相机两个位置归一化平面的投影（是需要通过像素坐标换算到世界坐标的，变换方法是通过相机的内参进行换算。归一化坐标为[X/Z,Y/Z,1]），只要大多数对极约束接近0，则说明计算的本质矩阵E 和 是正确的。也可以用计算出的R和T，用代替E ，验证T 和R的正确性。其中是李代数的部分内容，可以理解为矩阵的叉积。  是通过t生成的反对称矩阵。

源码有些地方写的有些麻烦，主要是为了锻炼自己多用vector的迭代器和引用传递，算是复习C++

附上源码（包含了特征点提取与匹配的部分）：

#include 
#include 
#include 


void PoseEstimation2d2d(std::vector,std::vector,cv::Mat&,cv::Mat&);
void EpipolarConstraint(cv::Mat,const std::vector&, const std::vector& , cv::Mat,std::vector&,std::vector&);

int main(){

    cv::Mat img1 = cv::imread("../1.png",1);
    cv::Mat img2 = cv::imread("../2.png",1);

    std::vector KeyPoint1,KeyPoint2;
    cv::Mat descriptors1,descriptors2;
    cv::Ptr detector = cv::ORB::create();//represented as vectors in a multidimensional space
    cv::Ptr descriptor = cv::ORB::create();//初始化描述子 descriptor为指针
    cv::Ptr matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

    detector->detect(img1,KeyPoint1);
    detector->detect(img2,KeyPoint2);

    descriptor->compute(img1,KeyPoint1,descriptors1);
    descriptor->compute(img2,KeyPoint2,descriptors2);

    // cv::Mat outimg1;
    // cv::drawKeypoints(img1,KeyPoint1,outimg1);
    // cv::imshow("img1特征点",outimg1);
    //可以看出descriptor描述子每个元素都是0-255 意味着二进制的编码
    //std::cout << descriptors1 << std::endl;
    std::vector matches;
    matcher->match(descriptors1,descriptors2,matches);
    //可以看到结果不理想，很多误匹配的点，需要设置阈值筛选
    // cv::Mat matchimg;
    // cv::drawMatches(img1,KeyPoint1,img2,KeyPoint2,matches,matchimg);
    // cv::imshow("特征匹配",matchimg);

    //迭代器遍历 for遍历也行
    double min = 10000,max = 0;
    min = min_element( matches.begin(), matches.end(), [](const cv::DMatch& m1, const cv::DMatch& m2) {return m1.distancedistance;
    max = max_element( matches.begin(), matches.end(), [](const cv::DMatch& m1, const cv::DMatch& m2) {return m1.distancedistance;

    std::vector GoodMatches;
    for(auto i = matches.begin(); i < matches.end(); i++){
        if(i->distance <= std::max( 2*min, 30.0 )){
            GoodMatches.push_back(*i);
        }
    }

    std::vector GoodMatchPoint1,GoodMatchPoint2;
    for(auto i = GoodMatches.begin(); i < GoodMatches.end(); i++){
        GoodMatchPoint1.push_back(KeyPoint1[i->queryIdx]);
        GoodMatchPoint2.push_back(KeyPoint2[i->trainIdx]);
    }
    cv::Mat R,t;
    PoseEstimation2d2d(GoodMatchPoint1,GoodMatchPoint2,R,t);

    cv::Mat t_x = ( cv::Mat_ ( 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 );
    cv::Mat t_r = t_x*R;
    std::cout << "t^R is " << std::endl << t_x*R << std::endl;

    // cv::Mat GoodMatchimg;
    // cv::drawMatches(img1,KeyPoint1,img2,KeyPoint2,GoodMatches,GoodMatchimg);
    // cv::imshow("特征匹配",GoodMatchimg);
    // cv::imwrite("../GoodResult.png", GoodMatchimg);
    // cv::waitKey(0);
    cv::Mat K = ( cv::Mat_ ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    cv::Point2d cam1;//点在归一化平面的坐标
    cv::Point2d cam2;

    std::vector x1;
    std::vector x2;

    EpipolarConstraint(t_r,GoodMatchPoint1,GoodMatchPoint2,K,x1,x2);
    

    return 0;
}

void EpipolarConstraint(cv::Mat E,const std::vector& PixelPoint1,const std::vector & PixelPoint2, cv::Mat K,std::vector & x1,std::vector & x2){
    cv::Point2d p1,p2;
    for(auto i = PixelPoint1.begin(); i < PixelPoint1.end(); i++){
        p1.x=(i->pt.x-K.at ( 0,2 ) ) / K.at ( 0,0 );
        p1.y=(i->pt.y-K.at ( 1,2 ) ) / K.at ( 1,1 );
        x1.push_back((cv::Mat_ ( 3,1 ) << p1.x, p1.y, 1)) ;
    }
    for(auto i = PixelPoint2.begin(); i < PixelPoint2.end(); i++){
        p2.x=(i->pt.x-K.at ( 0,2 ) ) / K.at ( 0,0 );
        p2.y=(i->pt.y-K.at ( 1,2 ) ) / K.at ( 1,1 );
        x2.push_back((cv::Mat_ ( 3,1 ) << p2.x, p2.y, 1)) ;
    }
    auto j = x2.begin();
    for(auto i = x1.begin(); i < x1.end(); i++,j++){
    cv::Mat EpipolarConstraint = (*j).t()* E * (*i);
    std::cout << "EpipolarConstraint is " <<  EpipolarConstraint << std::endl;
    }
}

void PoseEstimation2d2d(std::vector GoodMatchPoint1,std::vector GoodMatchPoint2,cv::Mat& R,cv::Mat& t){
    //初始化矩阵
    cv::Mat K = (cv::Mat_(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);//相机内参 fx fy cx cy

    std::vector points1, points2;
    //深拷贝
    for(auto i = GoodMatchPoint1.begin(); i < GoodMatchPoint1.end();i++){
        points1.push_back(i->pt) ;
    }

    for(auto i = GoodMatchPoint2.begin(); i < GoodMatchPoint2.end();i++){
        points2.push_back(i->pt) ;
    }
    cv::Mat FundamentalMat;
    FundamentalMat = cv::findFundamentalMat(points1,points2,cv::FM_8POINT);
    //std::cout << "fundamental_matrix is " << std::endl << FundamentalMat << std::endl;

    cv::Mat EssentialMat;
    EssentialMat = cv::findEssentialMat(points1,points2,K);
    //EssentialMat = cv::findEssentialMat(points1,points2,521.0, cv::Point2d(325.1,249.7));
    std::cout << "EssentialMat is " << std::endl << EssentialMat << std::endl;
    
    cv::recoverPose(EssentialMat,points1,points2,K,R,t);
    //cv::recoverPose(EssentialMat,points1,points2,R,t,521.0,cv::Point2d(325.1,249.7));

    //std::cout << "R is " << std::endl << R << std::endl;
    //std::cout << "t is " << std::endl << t << std::endl;


}

最终的运行结果如下：

EssentialMat is 
[0.0109715107420577, 0.2483381379213602, 0.03170694792954255;
 -0.2088597167616505, 0.029084710794909, -0.6744719771361425;
 0.008249595749295274, 0.6614170563251771, 0.01676713455468244]
t^R is 
[-0.01551605929362698, -0.3512031627022603, -0.04484039578292499;
 0.2953722440781573, -0.04113199246326703, 0.9538474175082218;
 -0.01166669019199051, -0.9353849714383189, -0.02371230909015907]
EpipolarConstraint is [-0.0005416579768821039]
EpipolarConstraint is [-0.002159398557490991]
EpipolarConstraint is [0.000329564098265081]
EpipolarConstraint is [-1.147072796144066e-05]
EpipolarConstraint is [0.000207988216714694]
EpipolarConstraint is [0.0001093677532205128]
EpipolarConstraint is [0.0004208746919946507]
EpipolarConstraint is [-0.003177997347355854]
EpipolarConstraint is [-4.07601554744362e-05]
EpipolarConstraint is [-0.00144829153347319]
EpipolarConstraint is [-0.0009615799940303968]
EpipolarConstraint is [-0.0008059848523957747]
EpipolarConstraint is [-0.001424066451101234]
EpipolarConstraint is [-0.0004308558850970052]
EpipolarConstraint is [-0.0004796564950435983]
EpipolarConstraint is [-0.0001958686247416352]
EpipolarConstraint is [0.001542044376943941]
EpipolarConstraint is [0.003106913282011514]
EpipolarConstraint is [0.0006736754289436708]
EpipolarConstraint is [-0.001173033197502501]
EpipolarConstraint is [-0.003987317225260016]
EpipolarConstraint is [-0.001256013301241379]
EpipolarConstraint is [-0.001550737161933333]
EpipolarConstraint is [0.00201042506321103]
EpipolarConstraint is [-0.0006111309111405394]
EpipolarConstraint is [-3.046189484311179e-05]
EpipolarConstraint is [0.001273590112099141]
EpipolarConstraint is [-0.004166758257779282]
EpipolarConstraint is [-0.001107404024207642]
EpipolarConstraint is [-0.0005148070022724105]
EpipolarConstraint is [-0.001776337642038897]
EpipolarConstraint is [-1.012294414959314e-12]
EpipolarConstraint is [-0.001856761858613207]
EpipolarConstraint is [-0.0004830391085845209]
EpipolarConstraint is [0.0004692803762148395]
EpipolarConstraint is [-0.002957773919832811]
EpipolarConstraint is [-0.003344265746093363]
EpipolarConstraint is [-0.0001976593466801457]
EpipolarConstraint is [-0.002823913457831666]
EpipolarConstraint is [-0.0004305282653053466]
EpipolarConstraint is [0.001181778913841636]
EpipolarConstraint is [-1.695033002846458e-12]
EpipolarConstraint is [-0.002141849714928215]
EpipolarConstraint is [0.001149598706735769]
EpipolarConstraint is [-0.002121385557104991]
EpipolarConstraint is [-1.019170858818086e-12]
EpipolarConstraint is [0.0009022844709607424]
EpipolarConstraint is [-0.002101173356852802]
EpipolarConstraint is [-0.0003542202672723524]
EpipolarConstraint is [-2.942426762718442e-05]
EpipolarConstraint is [-0.003786956691643206]
EpipolarConstraint is [-0.001587983371808432]
EpipolarConstraint is [-0.00292944464841105]
EpipolarConstraint is [-0.001016011288387075]
EpipolarConstraint is [0.0001215793621033301]
EpipolarConstraint is [-0.0009821639412199053]
EpipolarConstraint is [0.001458461868649605]
EpipolarConstraint is [-1.237312320791217e-05]
EpipolarConstraint is [-0.002735751536037237]
EpipolarConstraint is [0.001414873832845623]
EpipolarConstraint is [0.001789545990435903]
EpipolarConstraint is [-0.002550349147929159]
EpipolarConstraint is [-0.006256229913356795]
EpipolarConstraint is [-0.001875732926272605]
EpipolarConstraint is [-0.002105779810921735]
EpipolarConstraint is [-1.543598582287586e-12]
EpipolarConstraint is [-0.004014001023516178]
EpipolarConstraint is [-0.004726927213163874]
EpipolarConstraint is [-0.001328876731452905]
EpipolarConstraint is [5.292988269900434e-14]
EpipolarConstraint is [-0.005081445458945427]
EpipolarConstraint is [-0.001976431721038091]
EpipolarConstraint is [-0.001662302184417874]
EpipolarConstraint is [0.004283653601028871]
EpipolarConstraint is [0.004085015243819348]
EpipolarConstraint is [0.0001472315485530409]
EpipolarConstraint is [-0.0009652492684803365]
EpipolarConstraint is [-0.002341399165420976]
EpipolarConstraint is [-0.006138218209024097]

对于结果有几点注意

E 的 尺度是不确定的 这意味着E 乘以任何一个常数都是正确的 因为不会影响对极约束的结果，称为本质矩阵的尺度不确定性。可以看到结果中与E是差了一个常数项系数的。 

如果相机的所有特征点几乎共面，本质矩阵会退化，应该使用单应矩阵，这里没有写。