跟我一起学Multiple View Geometry多视图几何（5）编程实践课

　　前言：博主今天把Multiple View Geometry第九章后半部分也读完了，想着写点对应的代码练练手，程序基本思路如下：读入两张有一定视差的图片分别提取出ORB特征点以及描述子，再对描述子进行暴力匹配筛选获得若干goodmatches，然后调用OpenCV的findFundamentalMat函数计算F，再根据相机矩阵求出E，最后从E中恢复R and t 。

　　话不多说，上代码：

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

using namespace std;
using namespace cv;

int main()
{
    cout<<"Hello SLAM!"<<endl;
    cv::Mat img1,img2,img;
                                                              // first method
//    cv::Mat K = cv::Mat::zeros(3,3,CV_64FC1);      //cannot be 32 (float)
//    K.at<double>(0,0) = 518.0;
//    K.at<double>(1,1) = 519.0;
//    K.at<double>(0,2) = 325.5;
//    K.at<double>(1,2) = 253.5;
//    K.at<double>(2,2) = 1;

                                                              // second method , the most recommended
    cv::Matx33d K (518.0,    0,325.5,
                   0    ,519.0,253.5,
                   0    ,    0,    1);//calibration matrix
                                                              // Third method
//        cv::Matx33d K=cv::Matx33d(518.0,    0,325.5,
//                       0    ,519.0,253.5,
//                       0    ,    0,    1);//calibration matrix

                                                              // forth method
//    cv::Mat_<double> K = ( cv::Mat_<double>(3, 3) <<
//             518.0,    0,325.5,
//             0    ,519.0,253.5,
//             0    ,    0,    1);


    img1= cv::imread("/home/kylefan/program/cmake_demo/image/rgb1.png",IMREAD_COLOR);
    img2= cv::imread("/home/kylefan/program/cmake_demo/image/rgb2.png",IMREAD_COLOR);

    vector<cv::Point2f> point1;
    vector<cv::Point2f> point2;
    cv::ORB orb;
    vector<cv::KeyPoint>kp1,kp2;
    cv::Mat desp1,desp2;

    orb(img1,cv::Mat(),kp1,desp1,false);
    orb(img2,cv::Mat(),kp2,desp2,false);

    cout<<"分别找到了"<<kp1.size()<<"和"<<kp2.size()<<"个特征点"<<endl;

    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

    double knn_match_ratio = 0.3;
    vector< vector<cv::DMatch>> matches_knn;

    matcher->knnMatch(desp1,desp2,matches_knn,2);

    vector<cv::DMatch> goodmatches;

    for(int i=0;i<matches_knn.size();i++)
    {
        if(matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance)goodmatches.push_back(matches_knn[i][0]);
    }

    if(goodmatches.size()<20)
    {
        cout<<"too less goodmatches "<<endl;
    }

    for(auto m:goodmatches)
    {
        point1.push_back(kp1[m.queryIdx].pt);
        point2.push_back(kp2[m.trainIdx].pt);
    }
//    for(int j=0;j<goodmatches.size();j++)
//    {
//        //cout<<j<<":"<<point1[j].x<<","<<point1[j].y<<","<<point2[j].x<<","<<point2[j].y<<","<<endl;
//    }

    cv::Mat F = cv::findFundamentalMat(point1,point2,FM_RANSAC,3,0.99);//in calib3d

    //cv::Mat_<double> E = K.t() *F*  K;
    cv::Mat_<double> E = cv::Mat(K.t()) *F*  cv::Mat(K);//when you use the second method

    SVD svd(E);

    cv::Matx33d W(0,-1,0,
                  1,0,0,
                  0,0,1);

    cv::Mat_<double> R = svd.u * Mat(W) * svd.vt;
    cv::Mat_<double> t = svd.u.col(2);

    cv::Matx34d P1(R(0,0),R(0,1),R(0,2),t(0),
                   R(1,0),R(1,1),R(1,2),t(1),
                   R(2,0),R(2,1),R(2,2),t(2));

    cout<<goodmatches.size()<<" goodmatches "<<endl;

    cout<<"F="<<F<<endl;
    cout<<"K="<<K<<endl;
    cout<<"E="<<E<<endl;
    cout<<"R="<<R<<endl;
    cout<<"t="<<t<<endl;
    cout<<"P1="<<P1<<endl;

    if(fabsf(determinant(R))-1.0 > 1e-07)
    {
        cerr << "det(R) != +-1.0, this is not a rotation matrix" << endl;
    }
    else
    {
        cout<<"rotation matrix is right"<<endl;
    }
    return 0;
}

　　这里就是所有的实现代码，感觉思路还是很清晰的，不过在实现的时候还是与到了点小麻烦，主要的问题就是代码一开始给出的定义相机矩阵K的时候：
　　
　　第一个是直接定义成Mat型，并且指明行列数，但是由于未初始化其他几个为０的地方所以导致计算结果出错，另一个问题就是必须声明成和F一致的double型，因为opencv矩阵相乘要求类型完全一致才可以相乘：opencv-2.4.10/modules/core/src/matmul.cpp 711行：

CV_Assert( type == B.type() && (type == CV_32FC1 || type == CV_64FC1 || type == CV_32FC2 || type == CV_64FC2) );

　　第二个和第三个把K定义成了Matx33d型，它并不完全和Mat一样用，比如做矩阵乘法时候Matx33d要先强制转换成Mat型再与Mat相乘，直接相乘会报错，还有要注意的问题就是和上面一样，double才可以，Matx33f不行！

cv::Mat_<double> E = cv::Mat(K.t()) *F*  cv::Mat(K)//K是Matx33d型

　　第四个是定义成了Mat_型，这种东西我还是有点不清楚所以就不给大家过多解释了，我只知道在这里它是可以直接和Mat相乘的：
　　

cv::Mat_<double> E = K.t() *F*  K;//K是Mat_型

　　具体的请参考大神解释：http://blog.csdn.net/yhl_leo/article/details/47683127
　　和官网说明：http://docs.opencv.org/2.4/modules/core/doc/basic_structures.html
　　
　　总体来说，博主推荐第二种定义方法。既不会碰到漏初始化问题写起来也不麻烦。

　　下一个问题就是我们求出了形如下面的一个旋转矩阵：

R=[0.9584038608290109, -0.05325581227270088, -0.2804030278139331;
  0.0560243046922602, 0.9984276708501552, 0.001861011682810658;
  0.2798630322707793, -0.0174929854487338, 0.9598805543547047]

　　矩阵很清晰，就是怎么从矩阵里求出我们看开最直观的转轴和转角呢？

　　由于3D旋转都可以归结成按照某个单位向量n进行大小为θθ的旋转。所以，已知某个旋转时，可以推导出对应的旋转矩阵。该过程由罗德里格斯公式表明，由于过程比较复杂，我们在此不作赘述，只给出转换的结果（出自高翔博士视觉SLAM数学基础）：

　　
　　其中，然后就有：
　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　

　　关于转轴n，由于旋转轴上的向量在旋转后不发生改变，说明：
　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　

　　因此，只要求此方程的解向量即可。这也说明n是R特征值为1的一个特征向量。

　　有了这些理论知识，现在我们来求转轴和转角：

　　拿出matlab神器，新建R2theta.m文件

R=[0.9584038608290109  -0.05325581227270088  -0.2804030278139331;
  0.0560243046922602  0.9984276708501552  0.001861011682810658;
  0.2798630322707793  -0.0174929854487338  0.9598805543547047];
theta=acos(0.5*(trace(R)-1))*180/pi
[V,D]=eig(R)

　　输出：

theta =

   16.5933


V =

   0.7067 + 0.0000i   0.7067 + 0.0000i  -0.0339 + 0.0000i
  -0.0235 - 0.1354i  -0.0235 + 0.1354i  -0.9809 + 0.0000i
   0.0046 - 0.6940i   0.0046 + 0.6940i   0.1913 + 0.0000i


D =

   0.9584 + 0.2856i   0.0000 + 0.0000i   0.0000 + 0.0000i
   0.0000 + 0.0000i   0.9584 - 0.2856i   0.0000 + 0.0000i
   0.0000 + 0.0000i   0.0000 + 0.0000i   1.0000 + 0.0000i

　　D中特征值为1的第是三行三列的数，其对应的特征向量为V的第三列：
　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　

　　也就是说相机绕着转轴旋转了16.5933度。