ros机器人学习 --- SLAM-ORB特征匹配

一、图像准备

本次ORB特征匹配使用KITTI数据集。
将数据集上传到ubuntu系统中，并放置到一个文件夹下。

二、参考代码

在合适的文件夹下创建一个cpp文件，并在文件中写入c++代码：

#include <opencv2/opencv.hpp>
 #include <iostream>
#include <vector>
#include <string>
#include<sstream>
#include <fstream>
using namespace cv;
using namespace std;
// global variables

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<cv::KeyPoint> &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<bool> DescType;  // type of descriptor, 256 bools
void computeORBDesc(const cv::Mat &image, vector<cv::KeyPoint> &keypoints, vector<DescType> &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<DescType> &desc1, const vector<DescType> &desc2, vector<cv::DMatch> &matches);
 
int main(int argc, char **argv) {
   
    std::string pattern_jpg ="./kitti_Image/Kitti_image_2/*.png";
    std::vector<cv::String> image_files;
    cv::glob(pattern_jpg, image_files);
    for (unsigned int frame = 0; frame < image_files.size(); ++frame) {
   //image_file.size()代表文件中总共的图片个数
		//imshow("1", image);
		//imshow("2", image1);
		//waitKey(30);
          // load image
        cv::Mat first_image = cv::imread(image_files[frame], 0);    // load grayscale image
        cv::Mat second_image = cv::imread(image_files[frame+1], 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<cv::KeyPoint> keypoints;
        cv::FAST(first_image, keypoints, 40);
        cout << "keypoints: " << keypoints.size() << endl;
 
    // compute angle for each keypoint
        computeAngle(first_image, keypoints);
 
    // compute ORB descriptors
         vector<DescType> descriptors;
        computeORBDesc(first_image, keypoints, descriptors);
 
    // plot the keypoints
        cv::Mat image_show;
        cv::drawKeypoints(first_image, keypoints, image_show, cv::Scalar::all(-1),
                      cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
        string a="feature";
        string b=".png";
        stringstream ss;
        ss<<a<<frame<<b;
       // cv::imshow(ss.str(), image_show);
      
        cv::imwrite(ss.str(), image_show);
        cv::waitKey(0);
 
    // we can also match descriptors between images
    // same for the second
        vector<cv::KeyPoint> 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<DescType> descriptors2;
        computeORBDesc(second_image, keypoints2, descriptors2);
 
    // find matches
        vector<cv::DMatch> matches;
        bfMatch(descriptors, descriptors2, matches);
        if (matches.size()<4)
        {
   
            cout<<"匹配点过少!"<<endl;
        }
        cout << "matches: " << matches.size() << endl;
 
    // plot the matches
        cv::drawMatches(first_image, keypoints, second_image, keypoints2, matches, image_show);
        string c="matches";
        string d=".png";
        stringstream sb;
        sb<<c<<frame<<d;
        cv::imshow(sb.str(), image_show);
        cv::imwrite(sb.str(), image_show);
        cv::waitKey(0);
 
        cout << "done." << endl;
        cout<<endl;
	}
 
    return 0;
}
 
// -------------------------------------------------------------------------------------------------- //
 
// compute the angle
void computeAngle(const cv::Mat &image, vector<cv::KeyPoint> &keypoints) {
   
    int half_patch_size = 8;
    for (auto &kp : keypoints) {
   
    // START YOUR CODE HERE (~7 lines)
//        kp.angle = 0; // compute kp.angle
        double m10 = 0;
        double m01 = 0;
        int x =cvRound(kp.pt.x);
        int y =cvRound(kp.pt.y);
        if(x-half_patch_size<0||x+half_patch_size>image.cols||
           y-half_patch_size<0||y+half_patch_size>image.rows)
            continue;
        for(int u = x - half_patch_size;u<x + half_patch_size;++u)
        {
   
            for(int v = y -half_patch_size;v< y + half_patch_size;++v)
            {
   
                m10 +=  (u-x)*image.at<uchar>(v,u);
                m01 +=  (v-y)*image.at<uchar>(v,u);
            }
        }
 
        double theta = std::atan(m01/m10);
        kp.angle = theta * 180/pi;
        cout<<"kp.angel:"<<kp.angle<<endl;
//         END YOUR CODE HERE
    }
    return;
}
 
// -------------------------------------------------------------------------------------------------- //
// ORB pattern
int ORB_pattern[256 * 4] = {
   
        8, -3, 9, 5/*mean (0), correlation (0)*/,
        4, 2, 7, -12/*mean (1.12461e-05), correlation (0.0437584)*/,
        -11, 9, -8, 2/*mean (3.37382e-05), correlation (0.0617409)*/,
        7,