#include "Arith_FeaMatch.h"

// 构造函数
FeatureMatcher::FeatureMatcher(DetectorType detectorType, MatcherType matcherType)
    : detectorType_(detectorType), matcherType_(matcherType)
{
    initDetector();
    initMatcher();
}

// 提取特征点和描述符
void FeatureMatcher::extractFeatures(cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors)
{
    detector_->detectAndCompute(image, cv::noArray(), keypoints, descriptors);
}

// 匹配特征点
void FeatureMatcher::matchFeatures(cv::Mat& descriptors1, cv::Mat& descriptors2, std::vector<cv::DMatch>& matches)
{
    if (matcherType_ == FLANN)
    {
        flannMatcher_->match(descriptors1, descriptors2, matches);
    }
    else
    {
        bfMatcher_->match(descriptors1, descriptors2, matches);
    }
}

// 计算单应性矩阵
cv::Mat FeatureMatcher::computeHomography(std::vector<cv::KeyPoint>& keypoints1, std::vector<cv::KeyPoint>& keypoints2,
    std::vector<cv::DMatch>& matches, double ransacReprojThreshold)
{
    std::vector<cv::Point2f> points1, points2;
    for (const auto& match : matches)
    {
        points1.push_back(keypoints1[match.queryIdx].pt);
        points2.push_back(keypoints2[match.trainIdx].pt);
    }
    return cv::findHomography(points1, points2, cv::RANSAC, ransacReprojThreshold);
}

// 初始化特征检测器
void FeatureMatcher::initDetector()
{
    switch (detectorType_)
    {
    case SIFT:
        detector_ = cv::SIFT::create(100);
        break;
    case SURF:
        //detector_ = cv::xfeatures2d::SURF::create();
#ifdef HAVE_OPENCV_XFEATURES2D
       detector_ = cv::xfeatures2d::SURF::create();
#else
        throw std::runtime_error("SURF is not supported in this OpenCV build.");
#endif
        break;
    case ORB:
        detector_ = cv::ORB::create(1000);
        break;
    default:
        throw std::invalid_argument("Unsupported feature detector type");
    }
}

// 初始化匹配器
void FeatureMatcher::initMatcher()
{
    if (matcherType_ == FLANN)
    {
        if (detectorType_ == SIFT || detectorType_ == SURF)
        {
            flannMatcher_ = cv::FlannBasedMatcher::create();
        }
        else
        {
            flannMatcher_ = cv::makePtr<cv::FlannBasedMatcher>(cv::FlannBasedMatcher(cv::makePtr<cv::flann::LshIndexParams>(12, 20, 2)));
        }
    }
    else
    {
        if (detectorType_ == SIFT || detectorType_ == SURF)
        {
            bfMatcher_ = cv::BFMatcher::create(cv::NORM_L2);
        }
        else
        {
            bfMatcher_ = cv::BFMatcher::create(cv::NORM_HAMMING);
        }
    }
}