视觉slam十四讲 学习笔记-5

目录

特征点的提取和匹配

实践

2D-2D对极几何

对极约束

单应矩阵：

实践

 3D-2D PnP

特征点的提取和匹配

信息：位置、覆盖的大小、朝向等

通过在一个矩形中将亮度的值看做质量，然后去计算他的质心，然后通过图像中心和质心的连线就可以得出角点的方向，这样就得出了图像之间的旋转和匹配关系。

ORB 特征亦由关键点和描述子两部分组成：

 BRIEF描述子： 随机取多对坐标点（pi,qi），eg：如果p处的值大于q我们就记作1，否则记为0，这样多个点组成的一个1维矩阵就为BRIEF描述（通常为128对，即128*1的矩阵），这个描述是具有较好的鲁棒性的。下图就表示5种取pq点的方式（随机或者按照某种固定方式）。又因为上图中说的图像可能发生的旋转，我们就通过这个旋转对相应的旋转过后的图像的pq点也要进行类似的旋转，这样才可以保证前后两次的BRIEF基本一致。

汉明距离以美国数学家理查德·卫斯里·汉明的名字命名，表示两个相同长度字符串在相同位置上不同字符的个数。

 

图像金字塔：将某个图像的不同尺寸图像对应的特征求出，在图像进行缩放的时候，将缩放前后两个金字塔不同层的特征来进行匹配，从而解决缩放问题。

实践

首先将slambook_master中的ch7拷出来（也可以就在这里打开），注意这里的cmake_modules文件夹之前我还不理解，可以看这篇博客：

CMakeLists.txt使用cmake_module, list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)_awhuter的博客-CSDN博客_cmakelists.txt怎么用

然后打开代码文件夹，点击Run - edit configurations，修改Program arguments（Program arguments中是需要输入的命令行参数，即传入argv[]中的内容）和working directory后即可运行。

这里找了一个Fast特征点代码，和ORB类似：

int main(int argc,char* argv[])
{
	Mat srcImage = imread(".jpg");
	Mat srcGrayImage;
	if (srcImage.channels() == 3)
	{
		cvtColor(srcImage,srcGrayImage,CV_RGB2GRAY);
	}
	else
	{
		srcImage.copyTo(srcGrayImage);
	}
	vectordetectKeyPoint;
	Mat keyPointImage1,keyPointImage2;

	Ptr fast = FastFeatureDetector::create();
	fast->detect(srcGrayImage,detectKeyPoint);
	drawKeypoints(srcImage,detectKeyPoint,keyPointImage1,Scalar(0,0,255),DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
	drawKeypoints(srcImage,detectKeyPoint,keyPointImage2,Scalar(0,0,255),DrawMatchesFlags::DEFAULT);

	imshow("src image",srcImage);
	imshow("keyPoint image1",keyPointImage1);
	imshow("keyPoint image2",keyPointImage2);

	imwrite("F:\\opencv\\OpenCVImage\\FeatureDetectSrc1FASTKeyPointImageDefault.jpg",keyPointImage2);

	waitKey(0);
	return 0;
}

2D-2D对极几何

对极约束

现在,假设我们从两张图像中,得到了一对配对好的特征点,像图 7-7 里显示的那样。如果我们有若干对这样的匹配点,就可以通过这些二维图像点的对应关系,恢复出在两帧之间摄像机的运动

下图中：O1 O2表示的是不同时刻相机的位置，p点可以理解为空间中的一个真实的像素点，相机通过两帧的拍摄，将p点投影到各自的成像平面上即为p1、p2。

我们这里将作为世界坐标系的原点，所以有：。

由针孔相机模型归一化平面到像素坐标的转化

得出：

（s对应Z，p对应(u,v,1)）

我们去掉s深度信息得：

①：

设是两个像素在归一化平面上的坐标，所以有：

②：

将①②联立得：

同时叉乘t:

进一步左乘：

观察等式左侧,是一个与 t 和 x 2 都垂直的向量，所以左侧为0。因此,我们就得到了一个简洁的式子:

③：

将式②带入得：

④：

我们将，，得：

这两个式子都称为对极约束,它以形式简洁著名。它的几何意义是 O 1 , P, O 2 三者共
面。对极约束中同时包含了平移和旋转。我们把中间部分记作两个矩阵:基础矩阵(Fun-
damental Matrix)F 和本质矩阵(Essential Matrix)E

相机位姿估计问题变为以下两步:

求解E：

本质矩阵 E = t ∧ R。它是一个 3 × 3 的矩阵,内有 9 个未知数。由于平移和旋转各有三个自由度，故 t ∧ R 共有六个自由度，但由于尺度等价性，故 E 实际上有五个自由度。E 具有五个自由度的事实,表明我们最少可以用五对点来求解 E。但是,E 的内在性质是一种非线性性质，在求解线性方程时会带来麻烦，因此，也可以只考虑它的尺度等价性，使用八对点来估计 E——这就是经典的八点法(Eight-point-algorithm)

 

 

上图中只给出具体怎么算，原理不作深究 。  

 8点法求出E--->求出5点的E--->求出R、t

  

 单目相机初始化必须要有平移。RANSAC（随机采样一致性）来处理错误匹配的像素点

单应矩阵：

若场景中的特征点都落在同一平面上(比如墙,地面等)则可以通过单应性来进行运动估计，如果再去用E去做的话就会退化。

 上图中n是平面的法向量，d是相机系中原点到平面的距离

实践

其中find_feature_matches是上一个实践中的寻找匹配的特征点的内容

#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;
        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 "<

 3D-2D PnP

 1小时50分

---------作业----------

ORB 特征点：

提取FAST关键点：

    vector keypoints;     //定义存储关键点的容器，类型是KeyPoint
    cv::FAST(first_image, keypoints, 40);     //利用OpenCV内置的FAST函数，（图片、提取结果、阈值）
    cout << "keypoints: " << keypoints.size() << endl;

    //加入灰度质心角，成为orienteq FAST关键点，此时就具有旋转了不变性
    computeAngle(first_image, keypoints); //函数定义在下面给出

void computeAngle(const cv::Mat &image, vector &keypoints) {
    int half_patch_size = 8;
    for (auto &kp : keypoints) {
	// START YOUR CODE HERE (~7 lines)
        kp.angle = 0; // compute kp.angle 
        // END YOUR CODE HERE
    }
    return;
}

用于存储特征匹配的结果：

    vector matches;

ORB_pattern中记录每一行记录的是一对点相对于中心像素点的坐标。

总代码：

//
// Created by 高翔 on 2017/12/19.
// 本程序演示ORB是如何提取、计算和匹配的
//

#include 

#include 

using namespace std;

// global variables
string first_file = "/home/gzy/ROS/slam/14/ch5_homework_1_vs/1.png";
string second_file = "/home/gzy/ROS/slam/14/ch5_homework_1_vs/2.png";

const double pi = 3.1415926;    // pi


// TODO implement this function
/**
 * compute the angle for ORB descriptor
 * @param [in] image input image
 * @param [in|out] detected keypoints
 */
void computeAngle(const cv::Mat &image, vector &keypoints);

// TODO implement this function
/**
 * compute ORB descriptor
 * @param [in] image the input image
 * @param [in] keypoints detected keypoints
 * @param [out] desc descriptor
 */
typedef vector DescType;  // type of descriptor, 256 bools
void computeORBDesc(const cv::Mat &image, vector &keypoints, vector &desc);

// TODO implement this function
/**
 * brute-force match two sets of descriptors
 * @param desc1 the first descriptor
 * @param desc2 the second descriptor
 * @param matches matches of two images
 */
void bfMatch(const vector &desc1, const vector &desc2, vector &matches);

int main(int argc, char **argv) {

    // load image
    cv::Mat first_image = cv::imread(first_file, 0);    // load grayscale image
    cv::Mat second_image = cv::imread(second_file, 0);  // load grayscale image

    // plot the image
    cv::imshow("first image", first_image);
    cv::imshow("second image", second_image);
    cv::waitKey(0);

    // detect FAST keypoints using threshold=40
    vector keypoints;     //定义存储关键点的容器，类型是KeyPoint
    cv::FAST(first_image, keypoints, 40);     //利用OpenCV内置的FAST函数，（图片、提取结果、阈值）
    cout << "keypoints: " << keypoints.size() << endl;

    // compute angle for each keypoint
    //加入灰度质心角，成为orienteq FAST关键点，此时就具有旋转不变性
    computeAngle(first_image, keypoints);
    

    // compute ORB descriptors
    vector descriptors;
    computeORBDesc(first_image, keypoints, descriptors);  //最后存到descriptors中

    // plot the keypoints
    cv::Mat image_show;
    cv::drawKeypoints(first_image, keypoints, image_show, cv::Scalar::all(-1),
                      cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
    cv::imshow("features", image_show);
    cv::imwrite("feat1.png", image_show);
    cv::waitKey(0);

    // we can also match descriptors between images
    // same for the second
    vector keypoints2;
    cv::FAST(second_image, keypoints2, 40);
    cout << "keypoints: " << keypoints2.size() << endl;

    // compute angle for each keypoint
    computeAngle(second_image, keypoints2);


    // compute ORB descriptors
    vector descriptors2;
    computeORBDesc(second_image, keypoints2, descriptors2);

    // find matches
    vector matches;
    bfMatch(descriptors, descriptors2, matches);
    cout << "matches: " << matches.size() << endl;

    // plot the matches
    cv::drawMatches(first_image, keypoints, second_image, keypoints2, matches, image_show);
    cv::imshow("matches", image_show);
    cv::imwrite("matches.png", image_show);
    cv::waitKey(0);

    cout << "done." << endl;
    return 0;
}

// -------------------------------------------------------------------------------------------------- //

// compute the angle
void computeAngle(const cv::Mat &image, vector &keypoints) {
    int half_patch_size = 8;
    for (auto &kp : keypoints) {
	// START YOUR CODE HERE (~7 lines)
        kp.angle = 0; // compute kp.angle 
        float u=kp.pt.x;
        float v=kp.pt.y;
        float m10=0;
        float m01=0;

        //判断形成的图像块是否越界
        if(u-half_patch_size>=0 && v-half_patch_size>=0
            &&u+half_patch_size-1<=image.cols&&v+half_patch_size<=image.rows)
        {
            for (size_t i = -half_patch_size; i < half_patch_size; i++)
            {
                for (size_t j = -half_patch_size; j < half_patch_size; j++)
                {
                    m10+=(i)*image.at(v+j,u+i);
                    m01+=(j)*image.at(v+j,u+i);
                }    
            }
            kp.angle=atan2(m10,m01)/pi*180;
        }

        // END YOUR CODE HERE
    }
    cout<<"computeAngle done"< &keypoints, vector &desc) {
    for (auto &kp: keypoints) {  //取出一个特征点
        DescType d(256, false);
        for (int i = 0; i < 256; i++) {   //将特征点旁边的256对坐标来描述
            // START YOUR CODE HERE (~7 lines)
            d[i] = 0;  // if kp goes outside, set d.clear()
            double _sin=sin(kp.angle*pi/180);
            double _cos=cos(kp.angle*pi/180);
            cv::Point2f ORB_pattern_p(
               _cos* ORB_pattern[4*i]-_sin*ORB_pattern[4*i+1],
               _sin* ORB_pattern[4*i]+_cos*ORB_pattern[4*i+1]
            );
            cv::Point2f ORB_pattern_q(
               _cos* ORB_pattern[4*i+2]-_sin*ORB_pattern[4*i+3],
               _sin* ORB_pattern[4*i+2]+_cos*ORB_pattern[4*i+3]
            );
            cv::Point2f p=ORB_pattern_p+kp.pt;
            cv::Point2f q=ORB_pattern_q+kp.pt;
            //考虑是否越界
            if(p.x<0||p.y<0||p.y>image.cols||p.y>image.rows||
               q.x<0||q.y<0||q.y>image.cols||q.y>image.rows)
            {
                d.clear();
                break;
            }
            if(image.at(p)>image.at(q)){
                d[i]=0;
            }
            else{
                d[i]=1;
            }



	    // END YOUR CODE HERE
        }
        desc.push_back(d);
    }

    int bad = 0;
    for (auto &d: desc) {
        if (d.empty()) bad++;
    }
    cout << "bad/total: " << bad << "/" << desc.size() << endl;
    return;
} 

// brute-force matching
void bfMatch(const vector &desc1, const vector &desc2, vector &matches) {
    int d_max = 50;  //设置一个hamming距离的上限

    // START YOUR CODE HERE (~12 lines)
    for (size_t i = 0; i < desc1.size(); i++)
    {
        if(desc1[i].empty())  continue;
        //初始化
        int index2 = -1;
        int d_min = 256;
        for (size_t j = 0; j < desc2.size(); j++)
        {
            if(desc2[j].empty()) continue;
            int hamming_distance=0;
            for (size_t k = 0; k < 256; k++)
            {
                hamming_distance+=desc1[i][k]^desc2[j][k];
            }
            if(hamming_distance