#include <iostream>
#include <opencv2/highgui.hpp>
#include <opencv2/core.hpp>
#include <opencv2/xfeatures2d.hpp>

int main() {
    int64 t1, t2;
    double tkpt, tdes, tmatch_bf, tmatch_knn;

    // 1. 读取图片
    const cv::Mat image1 = cv::imread("../../images/1.png", 0); //Load as grayscale
    const cv::Mat image2 = cv::imread("../../images/2.png", 0); //Load as grayscale
    std::vector<cv::KeyPoint> keypoints1;
    std::vector<cv::KeyPoint> keypoints2;

    cv::Ptr<cv::SiftFeatureDetector> sift = cv::SiftFeatureDetector::create();
    // 2. 计算特征点
    t1 = cv::getTickCount();
    sift->detect(image1, keypoints1);
    t2 = cv::getTickCount();
    tkpt = 1000.0*(t2-t1) / cv::getTickFrequency();
    sift->detect(image2, keypoints2);


    // 3. 计算特征描述符
    cv::Mat descriptors1, descriptors2;
    t1 = cv::getTickCount();
    sift->compute(image1, keypoints1, descriptors1);
    t2 = cv::getTickCount();
    tdes = 1000.0*(t2-t1) / cv::getTickFrequency();
    sift->compute(image2, keypoints2, descriptors2);


    // 4. 特征匹配
    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create(cv::DescriptorMatcher::BRUTEFORCE);
    // cv::BFMatcher matcher(cv::NORM_L2);

    // (1) 直接暴力匹配
    std::vector<cv::DMatch> matches;
    t1 = cv::getTickCount();
    matcher->match(descriptors1, descriptors2, matches);
    t2 = cv::getTickCount();
    tmatch_bf = 1000.0*(t2-t1) / cv::getTickFrequency();
    // 画匹配图
    cv::Mat img_matches_bf;
    drawMatches(image1, keypoints1, image2, keypoints2, matches, img_matches_bf);
    imshow("bf_matches", img_matches_bf);

    // (2) KNN-NNDR匹配法
    std::vector<std::vector<cv::DMatch> > knn_matches;
    const float ratio_thresh = 0.7f;
    std::vector<cv::DMatch> good_matches;
    t1 = cv::getTickCount();
    matcher->knnMatch( descriptors1, descriptors2, knn_matches, 2);
    for (auto & knn_matche : knn_matches) {
        if (knn_matche[0].distance < ratio_thresh * knn_matche[1].distance) {
            good_matches.push_back(knn_matche[0]);
        }
    }
    t2 = cv::getTickCount();
    tmatch_knn = 1000.0*(t2-t1) / cv::getTickFrequency();

    // 画匹配图
    cv::Mat img_matches_knn;
    drawMatches( image1, keypoints1, image2, keypoints2, good_matches, img_matches_knn, cv::Scalar::all(-1),
                 cv::Scalar::all(-1), std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
    cv::imshow("knn_matches", img_matches_knn);
    cv::waitKey(0);



    cv::Mat output;
    cv::drawKeypoints(image1, keypoints1, output);
    cv::imwrite("sift_image1_keypoints.jpg", output);
    cv::drawKeypoints(image2, keypoints2, output);
    cv::imwrite("sift_image2_keypoints.jpg", output);


    std::cout << "图1特征点检测耗时(ms)：" << tkpt << std::endl;
    std::cout << "图1特征描述符耗时(ms)：" << tdes << std::endl;
    std::cout << "BF特征匹配耗时(ms)：" << tmatch_bf << std::endl;
    std::cout << "KNN-NNDR特征匹配耗时(ms)：" << tmatch_knn << std::endl;
    return 0;
}

cmake_minimum_required(VERSION 3.22)
project(demo)
find_package(OpenCV REQUIRED)

IF(UNIX)
    SET(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS} -g -O0  -Wall -Wextra -Wunused-variable -DDEBUG -D_DEBUG")
    SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -O0 -g  -Wall -Wextra -Wunused-variable -DDEBUG -D_DEBUG")
ENDIF(UNIX)


set(CMAKE_CXX_STANDARD 20)

add_executable(demo main.cpp)
target_link_libraries(demo ${OpenCV_LIBS})

cmake .
make
./demo