Challenge_4: 基于特征匹配的摄像头姿态估计

基于之前所做的特征匹配来实现摄像头姿态估计，即通过移动摄像头拍摄同一物体（如一本书）在不同姿态下的图片而计算出两次或者多次摄像头拍摄的位置。
要实现这一点，首先要对摄像头进行标定，获得摄像头的内参数，建立起摄像头的成像模型，才能够将图像物理坐标系（即照片中）中的点的信息与世界坐标系中的信息（即摄像头拍摄的位置）联系起来。
摄像头标定opencv已经提供了例程。
代码见。
https://github.com/opencv/opencv/blob/master/samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp
代码比较长，你们不需要具体弄懂这个代码。只需要会使用它。
官方文档的解释如下
http://docs.opencv.org/3.1.0/d4/d94/tutorial_camera_calibration.html
http://wiki.opencv.org.cn/index.php/%E6%91%84%E5%83%8F%E5%A4%B4%E6%A0%87%E5%AE%9A
基本原理的参考
http://www.xuebuyuan.com/1497516.html
标定的概念可以顺带了解一下，但对数学细节也不用太在意。
标定需要摄像头和棋盘格，可以自行打印。
对于用过Matlab的同学更推荐Caltech的标定工具箱，见
http://www.vision.caltech.edu/bouguetj/calib_doc/

在完成摄像机标定后可以获得摄像机内参数矩阵K，我们就可以与之前的特征检测结合起来，从摄像头不同角度位置拍摄同一本书的图像中获取摄像头的位置（即旋转平移矩阵）。
方法可以见下述博客一直到重构场景上面的内容。
http://blog.csdn.net/zkl99999/article/details/46467277
重点看找到相机矩阵小结。
方法是通过特征匹配的点，求出摄像机之间的基础矩阵，再与摄像机内参数矩阵K进行运算，算出本征矩阵。
通过对本征矩阵奇异值分解，可以求解出两张图对应的摄像机的旋转和平移矩阵，这也就代表了两张图拍摄间摄像机的相对运动。
大家也可以看到特征提取和匹配，摄像机运动估计确实用途特别广泛，是很多算法的先行步骤，包括进行三维重建也要先进行这些步骤。
大家首先要理解三个基本概念，即基础矩阵，本征矩阵以及奇异值分解。

基础矩阵（Fundamental Matrix）
是双视图几何中的一个概念，指的是当存在两个摄像头的情况，描述空间中某一点在两个摄像机像平面中成像点的位置关系，简单说是两张照片中对应特征点的图像坐标的关系，这种关系也被称为对极线约束。
两摄像机光心分别是C和C’，图像平面是两白色的平面，空间中某一个点X在两张图的投影点分别是x和x’。
(x’)T*F*x=0
假设已知像x和光心位置，我们通过连线可以得到空间点X可能在的一条射线位置，假设已知X在射线上某点，与另一个摄像头光心相连，在另一个像平面上可以得到一个交点，将点X在射线上滑动，可以在另一个像平面上得到一条线，这条线就是极线。同理，已知另一个像平面上的像x’,我们可以做出第一个像平面上的另一条极线。这就叫做对极线约束。
要注意的是基础矩阵只和摄像机的内外参数有关，点在空间任意位置都满足这个约束条件。

本征矩阵（Essential Matrix）
可以认为是在归一化图像坐标下的基本矩阵，考虑了摄像机内参数而进行归一化。通过本质矩阵和摄像机内参数矩阵K相乘可以得到基础矩阵。本征矩阵进行分解可以得到旋转和平移矩阵。可以认为本征矩阵描述了摄像机坐标系的变换关系，即同一个空间点在两个不同的摄像机坐标系中位置的约束关系。考虑到内参数矩阵将坐标从摄像机坐标系变换到了图像像素坐标系，所以就变成了两张不同的照片中点坐标的约束关系。

单应性矩阵
则是空间中平面上一点在两个像平面中的像的对应关系，知道其中一个像可以可得另一个像。但是要求存在这么一个平面，对凹凸不同的表面上的某空间点，当然也存在那么个矩阵描述它的两个像的对应关系，但是由于没有平面，你怎么求那个单应性矩阵（可以认为是该点的切平面的单应性矩阵）？因为我们最常用求单应性矩阵的方面是获得平面上一堆点在两个像平面中的像的对应关系再估计出来的。这一堆点既可以是棋盘格，也可以是随便什么特征点（比如SIFT 当然精度堪忧）。

可以看到调用opencv的函数很简单（除了各种中断和溢出的bug），困难的还在于理论，只有对这几个矩阵的概念有深入的了解，才知道如何通过特征匹配来估计摄像头的位置和姿态。
以下是代码，通过同一摄像头拍摄的一本书的两张图像进行SIFT特征提取，FLAN特征匹配来得到特征点对。通过特征点估计基础矩阵，并最终求得摄像头在两次拍摄中运动的旋转和平移矩阵，从而进行摄像头姿态估计。要注意的是矩阵相乘对数据类型的要求。所以这里使用了Converto成员函数。

#include <stdio.h>#include <iostream>#include "opencv2/core.hpp"#include "opencv2/imgproc.hpp"#include "opencv2/features2d.hpp"#include "opencv2/highgui.hpp"#include "opencv2/calib3d.hpp"#include "opencv2/xfeatures2d.hpp"using namespace std;using namespace cv;using namespace cv::xfeatures2d;void readme();/* @function main */int main(){    Mat img_object = imread("a.jpg", IMREAD_GRAYSCALE);    Mat img_scene = imread("b.jpg", IMREAD_GRAYSCALE);    if (!img_object.data || !img_scene.data)    {        std::cout << " --(!) Error reading images " << std::endl; return -1;    }    Ptr<SIFT > detector = SIFT::create();    std::vector<KeyPoint> keypoints_object, keypoints_scene;    Mat descriptors_object, descriptors_scene;    detector->detectAndCompute(img_object, Mat(), keypoints_object, descriptors_object);    detector->detectAndCompute(img_scene, Mat(), keypoints_scene, descriptors_scene);    FlannBasedMatcher matcher;    std::vector< DMatch > matches;    matcher.match(descriptors_object, descriptors_scene, matches);    double max_dist = 0; double min_dist = 100;    //-- Quick calculation of max and min distances between keypoints    for (int i = 0; i < descriptors_object.rows; i++)    {        double dist = matches[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);    std::vector< DMatch > good_matches;    for (int i = 0; i < descriptors_object.rows; i++)    {        if (matches[i].distance < 3 * min_dist)        {            good_matches.push_back(matches[i]);        }    }    Mat img_matches;    drawMatches(img_object, keypoints_object, img_scene, keypoints_scene,        good_matches, img_matches, Scalar::all(-1), Scalar::all(-1),        std::vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);    //-- Localize the object    std::vector<Point2f> obj;    std::vector<Point2f> scene;    for (size_t i = 0; i < good_matches.size(); i++)    {        //-- Get the keypoints from the good matches        obj.push_back(keypoints_object[good_matches[i].queryIdx].pt);        scene.push_back(keypoints_scene[good_matches[i].trainIdx].pt);    }    Mat F = findFundamentalMat(obj, scene, FM_RANSAC, 0.1, 0.99);    Mat F2;    F.convertTo(F2, CV_32FC1);    Mat K(3,3, CV_32FC1);    K.at<float>(0,0) = 1.08e3;    K.at<float>(0,1) = 0;    K.at<float>(0,2) = 4.8e2;    K.at<float>(1,0) = 0;    K.at<float>(1,1) = 1.09e3;    K.at<float>(1,2) = 6.4e2;    K.at<float>(2,0) = 0;    K.at<float>(2,1) = 0;    K.at<float>(2,2) = 1;    cout << K;    cout << "\n";    cout << F;    cout << "\n";    cout << F2;    Mat_<double> E = K.t() * F2 * K;    cout << "\n";    cout << E;    SVD svd(E);    Matx33d W(0, -1, 0,        1, 0, 0,        0, 0, 1);    Mat_<double> R = svd.u * Mat(W) * svd.vt;     Mat_<double> t = svd.u.col(2);    cout << "\n" << E << "\n" << t;    return 0;}