#include "feature_extractor.hpp"

#include <algorithm>
#include <iterator>

#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp> 

#ifdef OPENCV_CONTRIB
#include <opencv2/xfeatures2d.hpp>
#endif
#include <opencv2/video/tracking.hpp>

//作用是为OpenCV提供一种方便的方式来使用Lambda函数作为并行处理函数的输入，提高程序的并行化效率。
#ifdef OPENCV_LAMBDA_MISSING        //条件编译指令，意思是如果OPENCV_LAMBDA_MISSING没有被定义，那么就编译这段代码。

namespace cv {
    
    //这段代码定义了一个名为cv::ParallelLoopBodyLambdaWrapper的类，它继承自ParallelLoopBody，并在其基础上封装了一个lambda函数。
    //这个类的作用是将用户传入的函数对象（functor）转换成OpenCV并行处理函数所需的ParallelLoopBody对象。
  class ParallelLoopBodyLambdaWrapper : public ParallelLoopBody
  {
    private:
      std::function<void(const Range&)> m_functor;
    public:
      ParallelLoopBodyLambdaWrapper(std::function<void(const Range&)> functor) :
        m_functor(functor)
        { }    
      virtual void operator() (const cv::Range& range) const
        {
          m_functor(range);
        }
  };
  
  inline void parallel_for_(const cv::Range& range, std::function<void(const cv::Range&)> functor, double nstripes=-1.)
    {
      parallel_for_(range, ParallelLoopBodyLambdaWrapper(functor), nstripes);
    }
}

#endif

/*
这段代码使用了OpenCV库中的特征检测和描述子提取算法。
定义了三个指向特征检测和描述子提取器的指针，分别为pgftt_、pfast_和pbrief_。
这些指针使用了智能指针类cv::Ptr，以确保在使用完毕后正确地释放内存资源。
pgftt_指向了GFTT（Good Features to Track）特征检测器，pfast_指向了FAST（Features from Accelerated Segment Test）特征检测器。
pbrief_则指向了Brief描述子提取器，但是由于OpenCV的版本不同，它可能指向cv::DescriptorExtractor或cv::xfeatures2d::BriefDescriptorExtractor。
*/
cv::Ptr<cv::GFTTDetector> pgftt_;
cv::Ptr<cv::FastFeatureDetector> pfast_;
#ifdef OPENCV_CONTRIB
cv::Ptr<cv::xfeatures2d::BriefDescriptorExtractor> pbrief_;
#else
cv::Ptr<cv::DescriptorExtractor> pbrief_;
#endif

//定义了一个名为compare_response的函数，它接受两个cv::KeyPoint对象作为输入，并返回一个布尔值。
//该函数通过比较两个KeyPoint对象的响应值(response)大小，决定它们在排序中的先后顺序。
bool compare_response(cv::KeyPoint first, cv::KeyPoint second) {
    return first.response > second.response;
}

/*
成员变量nmaxpts_表示在图像中提取的最大关键点数，nmaxdist_表示关键点之间的最大距离，
dmaxquality_表示提取的关键点的最大质量值，nfast_th_表示用于快速提取关键点的阈值。
*/
FeatureExtractor::FeatureExtractor(size_t nmaxpts, size_t nmaxdist, double dmaxquality, int nfast_th)
    : nmaxpts_(nmaxpts), nmaxdist_(nmaxdist), dmaxquality_(dmaxquality), nfast_th_(nfast_th)
{
    nmindist_ = nmaxdist / 2.;
    dminquality_ = dmaxquality / 2.;

    //打印了一些关于Feature Extractor的信息，包括最大关键点数、最大关键点距离、最小关键点距离、最大关键点质量值和最小关键点质量值。
    std::cout << "\n*********************************\n";
    std::cout << "\nFeature Extractor is constructed!\n";
    std::cout << "\n>>> Maximum nb of kps : " << nmaxpts_;
    std::cout << "\n>>> Maximum kps dist : " << nmaxdist_;
    std::cout << "\n>>> Minimum kps dist : " << nmindist_;
    std::cout << "\n>>> Maximum kps qual : " << dmaxquality_;
    std::cout << "\n>>> Minimum kps qual : " << dminquality_;
    std::cout << "\n*********************************\n";
}

/**
 * \brief Detect GFTT features (Harris corners with Shi-Tomasi method) using OpenCV.
 *
 * \param[in] im  Image to process (cv::Mat).
 * \param[in] vcurkps Vector of px positions to filter out detections.
 * \param[in] roi  Initial ROI mask (255 ok / 0 reject) to filter out detections (cv::Mat<CV_8UC1>).
 * \return Vector of deteted kps px pos.
 */
//这段代码是特征提取器FeatureExtractor中的函数detectGFTT，它使用Good Features to Track (GFTT)算法检测输入图像中的角点，并返回检测到的角点的像素坐标。
//函数的输入参数包括：待检测的图像im、已经检测到的关键点的像素坐标vcurkps、感兴趣区域roi（可选）、最大检测数目nbmax（可选）；
//输出参数为一个std::vectorcv::Point2f类型的向量，其中存储了检测到的角点的像素坐标。

/*
函数实现的主要步骤如下：

1.首先计算需要检测的角点数目，如果已经检测的关键点数目已经超过了最大数目nmaxpts_，则返回一个空的向量；
如果用户指定了最大检测数目nbmax，则使用nbmax作为需要检测的角点数目。

2.初始化掩膜mask，如果用户提供了感兴趣区域roi，则将其拷贝到mask中；调用setMask函数，
该函数将mask中已经存在的关键点区域和nmaxdist_参数指定的距离内的区域设置为0，以避免检测到相似的角点；
然后使用cv::GFTTDetector检测图像中的角点，并将检测到的角点存储在vnewkps中。

3.将检测到的关键点转换成像素坐标，并将其存储在vnewpts中。如果检测到的角点数目大于0，则使用cv::cornerSubPix函数对其进行亚像素级别的精细化处理。

4.如果检测到的角点数目超过了0.66倍的需要检测的角点数目，或者需要检测的角点数目小于20，则直接返回检测到的角点。
否则，计算还需要检测的角点数目，并更新掩膜mask以强制检测那些不在当前关键点区域或已经检测到的角点周围nminidist_像素范围内的区域；
重新使用cv::GFTTDetector检测图像中的角点，存储在vnewkps中；将检测到的角点转换成像素坐标并存储在vmorepts中；
如果检测到的角点数目大于0，则使用cv::cornerSubPix函数对其进行亚像素级别的精细化处理。

5.将vmorepts中的角点像素坐标插入到vnewpts中，并返回vnewpts。
*/
std::vector<cv::Point2f> FeatureExtractor::detectGFTT(const cv::Mat &im, const std::vector<cv::Point2f> &vcurkps, const cv::Mat &roi, int nbmax) const
{
    // 1. Check how many kps we need to detect计算需要检测的角点数目
    //初始化要检测的特征点数量。如果已经检测到的特征点数大于等于 nmaxpts_，那么直接返回空的 std::vector。
    size_t nb2detect = nmaxpts_ - vcurkps.size();
    if( vcurkps.size() >= nmaxpts_ ) {
        return std::vector<cv::Point2f>();
    }

    //如果指定了 nbmax，那么使用该值来覆盖 nb2detect。
    if( nbmax != -1 ) {
        nb2detect = nbmax;
    }

    // 1.1 Init the mask
    //初始化二值掩码 mask，如果提供了 roi，则使用其副本，然后调用 setMask 函数，以检查并修剪在已检测到的特征点周围的区域。
    cv::Mat mask;
    if( !roi.empty() ) {
        mask = roi.clone();
    }
    setMask(im, vcurkps, nmaxdist_, mask);

    // 1.2 Extract kps
    /*
    使用 C++ 和 OpenCV 库来定义并初始化两个空的 vector 对象，即 vnewkps 和 vnewpts
    cv::KeyPoint 是 OpenCV 库中表示关键点的结构体，包含位置、尺度、方向等信息
    cv::Point2f 是 OpenCV 库中表示二维坐标点的结构体，包含 x 和 y 坐标信息

    vnewkps.reserve(nb2detect) 和 vnewpts.reserve(nb2detect) 分别调用了 vector 对象的 reserve() 方法，用于预分配存储空间。
    这里 nb2detect 预估需要存储的元素数量，通过预分配存储空间可以避免在插入元素时频繁进行内存重新分配，提高程序的效率。
    但需要注意的是，调用 reserve() 方法并不会改变 vector 对象的大小，只是为其预分配足够的空间。
    */
    std::vector<cv::KeyPoint> vnewkps;
    std::vector<cv::Point2f> vnewpts;
    vnewkps.reserve(nb2detect);
    vnewpts.reserve(nb2detect);

    // std::cout << "\n*****************************\n";
    // std::cout << "\t\t GFTT \n";
    // std::cout << "\n> Nb max pts : " << nmaxpts_;
    // std::cout << "\n> Nb 2 detect : " << nb2detect;
    // std::cout << "\n> Quality : " << dminquality_;
    // std::cout << "\n> Dist : " << nmaxdist_;

    // Init. detector if not done yet
    if( pgftt_ == nullptr )     //检查指针“pgftt_”是否等于nullptr，这意味着它尚未初始化。
    {
        //如果是这种情况，代码将使用参数“nmaxpts_”、“dminquality_”和“nmaxdist_”创建一个新的GFTTDetector类实例，并将其赋值给“pgftt_”
        //这些参数控制最多检测关键点的数量（“nmaxpts_”）、被视为关键点的最小质量水平（“dminquality_”）以及两个关键点之间的最小距离（“nmaxdist_”）
        pgftt_ = cv::GFTTDetector::create(nmaxpts_, dminquality_, nmaxdist_);
    }
    
    //使用“pgftt_”对象的成员函数设置GFTT检测器参数
    //“setQualityLevel”函数设置被视为关键点的最小质量水平，“setMinDistance”函数设置两个关键点之间的最小距离。“setMaxFeatures”函数设置最多检测关键点的数量。
    pgftt_->setQualityLevel(dminquality_);
    pgftt_->setMinDistance(nmaxdist_);
    pgftt_->setMaxFeatures(nb2detect);
    //使用“pgftt_”对象的“detect”函数将GFTT检测器应用于输入图像“im”。
    //检测到的关键点存储在“vnewkps”向量中，可选的“mask”参数可用于指定二进制掩码，指示应将图像的哪些像素视为关键点检测的区域。
    pgftt_->detect(im, vnewkps, mask);

    //这个函数是用来根据像素掩模过滤特征点的，其中vnewkps是一个输入参数，代表输入的特征点向量，mask是一个输入参数，代表像素掩模。
    // 这个函数会在vnewkps中保留那些位于掩模内的特征点，并将其余特征点过滤掉。
    // cv::KeyPointsFilter::runByPixelsMask(vnewkps,mask);
    //这个函数将vnewkps中的特征点转换为点向量vnewpts。其中，vnewkps是一个输入参数，代表需要转换的特征点向量，vnewpts是一个输出参数，代表输出的点向量。
    cv::KeyPoint::convert(vnewkps, vnewpts);
    vnewkps.clear();//清空vnewkps向量，释放内存空间。

    // Check number of detections检查检测次数
    size_t nbdetected = vnewpts.size();//获取 vector 容器中点的数量
    // std::cout << "\n \t>>> Found : " << nbdetected;

    // Compute Corners with Sub-Pixel Accuracy以亚像素精度计算角点
    if( nbdetected > 0 )//判断是否检测到了角点（nbdetected 大于0）。如果检测到了角点，则使用 OpenCV 库中的 cornerSubPix 函数来优化角点的位置。
    {
        /// Set the need parameters to find the refined corners设置需要的参数来寻找细化的角点
        cv::Size winSize = cv::Size(3,3);
        cv::Size zeroZone = cv::Size(-1,-1);
        cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::EPS + 
                                        cv::TermCriteria::MAX_ITER, 30, 0.01);

        /*
        cornerSubPix 函数会优化图像中检测到的角点的位置，使其更加准确，能够达到亚像素级别的精度。该函数需要以下参数：
            im：输入的图像，即包含角点的图像。
            vnewpts：一个二维点向量，表示检测到的角点的位置。经过函数调用后，优化后的角点位置将存储在该向量中。
            winSize：优化过程中使用的窗口大小。较大的窗口大小可以提高角点位置的精度，但也会增加计算时间。
            zeroZone：搜索区域中心的死区大小。该参数可用于避免过多的优化迭代次数，从而避免不稳定的结果。
            criteria：优化过程的终止条件。如果达到了最大迭代次数 (MAX_ITER) 或所需的精度 (EPS)，则优化过程将停止。
        */
        cv::cornerSubPix(im, vnewpts, winSize, zeroZone, criteria);
    }

    // If enough, return kps
    //如果检测到的数量超过了要检测的数量的0.66倍，则条件为真
    //nb2detect < 20：这个条件表示如果要检测的数量小于20，则条件为真
    if( nbdetected >= 0.66 * nb2detect || nb2detect < 20 ) {
        return vnewpts;
    }

    // Else, detect more
    /*
    这段程序代码的作用是从 nb2detect 中减去已检测到的数量 nbdetected，然后创建一个名为 vmorepts 的空向量，
    该向量预留了足够的空间以存储 nb2detect 个元素。
    */
    nb2detect -= nbdetected;//计算还需要检测的点数
    std::vector<cv::Point2f> vmorepts;//定义了一个名为 vmorepts 的新的 std::vector 对象，用于存储更多的 cv::Point2f 类型的数据
    vmorepts.reserve(nb2detect);//预留空间，以便在 vmorepts 向量中存储 nb2detect 个元素，避免多次调整向量的大小，从而提高程序的效率。

    // Update mask to force detection around 更新掩码以强制检测未观察到的区域
    // not observed areas
    mask.release();//释放先前创建的掩膜
    if( !roi.empty() ) {
        mask = roi.clone();//判断感兴趣区域（roi）是否为空。如果不为空，就将其克隆到掩膜中
    }
    setMask(im, vcurkps, nmindist_, mask);//然后使用 setMask 函数，对图像的特定区域进行掩膜操作。vcurkps（当前关键点）
    setMask(im, vnewpts, nmindist_, mask);//vnewpts（新的关键点）

    // Detect additional kps
    // std::cout << "\n \t>>>  Searching more : " << nb2detect;

    pgftt_->setQualityLevel(dmaxquality_);//设置最大特征质量级别（dmaxquality_），该级别确定了在图像中选择哪些关键点。值越大，选择的关键点越少。
    pgftt_->setMinDistance(nmindist_);//设置关键点之间的最小距离（nmindist_）。该距离决定了在图像中选择关键点的密度。距离越大，密度越小。
    pgftt_->setMaxFeatures(nb2detect);//设置最大特征数量（nb2detect），即在图像中选择多少个关键点。如果图像中有更多的特征点，则只会选择排名前面的 nb2detect 个关键点。
    pgftt_->detect(im, vnewkps, mask);//检测关键点。输入参数包括原始图像 im、输出参数 vnewkps（新检测到的关键点）和mask（可以选择性地将关键点限制在指定的区域内）。

    cv::KeyPoint::convert(vnewkps, vmorepts);//将一个存储在vnewkps中的关键点向量转换为一个存储在vmorepts中的点向量。
    vnewkps.clear();//清空了vnewkps向量中的数据

    nbdetected = vmorepts.size();//nbdetected变量被赋值为vmorepts向量的大小，即检测到的点的数量
    // std::cout << "\n \t>>>  Additionally found : " << nbdetected;

    // Compute Corners with Sub-Pixel Accuracy以亚像素精度计算角点
    if( nbdetected > 0 )//如果检测到的角点数量大于0
    {
        /// Set the need parameters to find the refined corners设置需要的参数来寻找细化的角点
        cv::Size winSize = cv::Size(3,3);//设置窗口大小,定义了用于寻找角点的局部窗口的大小
        cv::Size zeroZone = cv::Size(-1,-1);//设置零区域,定义了计算亚像素级别角点时使用的搜索区域大小
        cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::EPS + 
                                        cv::TermCriteria::MAX_ITER, 30, 0.01);//设置停止准则,定义了迭代过程的停止条件，这里使用的是最大迭代次数和epsilon值

        cv::cornerSubPix(im, vmorepts, winSize, zeroZone, criteria);//调用cv::cornerSubPix()函数来对图像(im)中的角点进行亚像素级别的优化，结果存储在vmorepts中。

    }

    // Insert new detections插入新的检测
    /*
    将一个名为 "vmorepts" 的向量中的元素移动到另一个名为 "vnewpts" 的向量的末尾。这是通过使用 std::make_move_iterator() 函数实现的，
    该函数将迭代器转换为移动迭代器，使得移动语义得以使用。移动迭代器在移动元素时更高效，因为它们不会进行复制，
    而是将对象的所有权从一个容器转移到另一个容器，从而减少了不必要的内存分配和释放。
    */
    vnewpts.insert(vnewpts.end(), 
                    std::make_move_iterator(vmorepts.begin()),
                    std::make_move_iterator(vmorepts.end())
                );

    // std::cout << "\n \t>>>  Total found : " << vnewpts.size();

    // Return vector of detected kps px pos.
    return vnewpts;
}

/*
定义了一个名为 FeatureExtractor 的类的成员函数 describeBRIEF，该函数接受一张图像和一组二维坐标点，
然后使用 BRIEF 或 ORB 描述符提取算法提取这些关键点的特征描述。
该函数返回一个包含特征描述的 Mat 类型的向量，每个元素对应一个输入坐标点的特征描述。
*/
std::vector<cv::Mat> FeatureExtractor::describeBRIEF(const cv::Mat &im, const std::vector<cv::Point2f> &vpts) const
{
    if( vpts.empty() ) {
        // std::cout << "\nNo kps provided to function describeBRIEF() \n";
        return std::vector<cv::Mat>();//如果传入的坐标点向量为空，则该函数返回一个空向量
    }

    std::vector<cv::KeyPoint> vkps;//将2D关键点转换为cv::KeyPoint对象(vkps)，并为特征描述符向量(vdescs)和vkps向量预留存储空间。
    size_t nbkps = vpts.size();//计算特征点的数量
    vkps.reserve(nbkps);//调用vkps向量的reserve函数和vdescs向量的reserve函数来预留存储空间。提高代码的效率
    std::vector<cv::Mat> vdescs;
    vdescs.reserve(nbkps);

    cv::KeyPoint::convert(vpts, vkps);//将2D关键点的坐标向量(vpts)转换为KeyPoint对象向量(vkps)

    cv::Mat descs;//该函数声明一个Mat类型的变量descs，该变量用于存储计算出的特征描述符。

    //如果在调用函数时，类成员变量pbrief_的值为nullptr，说明当前未创建BRIEF特征描述符提取器，则程序会尝试创建一个。
    /*
    由于OpenCV的contrib模块是可选的，因此使用BRIEF算法需要开启contrib模块。
    如果未开启contrib模块，则上述代码将输出一条消息，说明BRIEF算法无法使用，并将使用ORB算法作为替代方案。
    不过，ORB算法不能实现旋转和尺度不变性。
    */
    if( pbrief_ == nullptr ) {
        #ifdef OPENCV_CONTRIB
        pbrief_ = cv::xfeatures2d::BriefDescriptorExtractor::create();//如果该模块被编译，则会创建BriefDescriptorExtractor实例；否则，代码将使用ORB算法作为替代方案。
        #else
        /*
        500：OR算法检测到的关键点的最大数量。此值可以根据应用程序的要求进行调整，以控制检测到的关键点数量。
        1.0：用于检测关键点的图像金字塔中图层之间的比例因子。较小的值将在较细的尺度上检测关键点，而较大的值将在较粗的尺度上检测关键点。
        0：图像金字塔中的级别数。值为0意味着ORB算法仅应用于原始图像而没有任何降采样。
        */
        pbrief_  = cv::ORB::create(500, 1., 0);
        std::cout << "\n\n=======================================================================\n";
        std::cout << " BRIEF CANNOT BE USED ACCORDING TO CMAKELISTS (Opencv Contrib not enabled) \n";
        std::cout << " ORB WILL BE USED INSTEAD!  (BUT NO ROTATION  OR SCALE INVARIANCE ENABLED) \n";
        std::cout << "\n\n=======================================================================\n\n";
        #endif
    }

    // std::cout << "\nCOmputing desc for #" << vkps.size() << " kps\n";

    pbrief_->compute(im, vkps, descs);//输出参数descs是一个矩阵，它包含了计算出的关键点描述符。

    // std::cout << "\nDesc computed for #" << vkps.size() << " kps\n";

    //计算出的关键点向量vkps是否为空,如果是，说明没有检测到任何关键点，因此函数返回一个大小为nbkps的空矩阵向量。
    if( vkps.empty() ) {
        return std::vector<cv::Mat>(nbkps, cv::Mat());
    }

    /*
    这段代码将之前计算出的关键点描述符矩阵descs按照输入的关键点位置vpts进行筛选，
    将与输入关键点匹配的描述符加入到一个新的描述符矩阵中，不匹配的则用空矩阵代替。
    */
    size_t k = 0;
    for( size_t i = 0 ; i < nbkps ; i++ ) 
    {
        if( k < vkps.size() ) {
            /*
            检查当前关键点位置vpts[i]是否等于vkps[k].pt，如果是，则将对应的描述符添加到新的描述符矩阵vdescs中。
            具体地，将descs.row(k)的一行复制并添加到vdescs的末尾，并将指针k向下移动一行。如果当前关键点位置vpts[i]与vkps[k].pt不匹配，
            则将一个空矩阵对象cv::Mat()添加到vdescs的末尾。这样，如果没有找到与当前关键点位置匹配的描述符，则在vdescs中添加一个空的矩阵对象。
            */
            if( vkps[k].pt == vpts[i] ) {
                // vdescs.push_back(descs.row(k).clone());
                vdescs.push_back(descs.row(k));
                k++;
            }
            else {
                vdescs.push_back(cv::Mat());
            }
        } else {
            vdescs.push_back(cv::Mat());
        }
    }

    assert( vdescs.size() == vpts.size() );//如果vdescs的大小与vpts的大小不同，则程序将崩溃并抛出一个断言错误。

    // std::cout << "\n \t >>> describeBRIEF : " << vkps.size() << " kps described!\n";

    return vdescs;
}

/*
用于特征提取。此函数使用一个标准的图像金字塔框架（尺度空间），在一定的图像尺度上检测出图像中的角点。
函数接受输入参数包括输入图像im，单元格尺寸ncellsize，当前帧的关键点vcurkps，和检测区域roi，并返回检测到的角点的向量std::vector<cv::Point2f>
*/
std::vector<cv::Point2f> FeatureExtractor::detectSingleScale(const cv::Mat &im, const int ncellsize, 
        const std::vector<cv::Point2f> &vcurkps, const cv::Rect &roi) 
{   
    //1.检查输入的图像是否为空，如果是，返回一个空的向量std::vector<cv::Point2f>()
    if( im.empty() ) {
        // std::cerr << "\n No image provided to detectSingleScale() !\n";
        return std::vector<cv::Point2f>();
    }

    //2.获取输入图像的行数和列数，计算单元格内部一半的长度nhalfcell，以及水平和垂直方向的单元格数量nhcells和nwcells，以及总单元格数量nbcells
    size_t ncols = im.cols;
    size_t nrows = im.rows;

    size_t nhalfcell = ncellsize / 4;

    size_t nhcells = nrows / ncellsize;
    size_t nwcells = ncols / ncellsize;

    size_t nbcells = nhcells * nwcells;

    //3.这两个容器一起被用于在网格中存储检测到的点。vdetectedpx是一个一维向量，存储2D点，
    //voccupcells是一个二维向量，存储布尔值，指示网格中的每个单元格是否被占用。网格的维度由nhcells和nwcells的值确定。
    std::vector<cv::Point2f> vdetectedpx;
    vdetectedpx.reserve(nbcells);

    std::vector<std::vector<bool>> voccupcells(
            nhcells+1, 
            std::vector<bool>(nwcells+1, false)//所有单元格初始化为“未占用”
            );
    
    //使用了OpenCV库中的cv::Mat类来创建一个名为mask的矩阵，它是一个大小为im.rows x im.cols的单通道32位浮点数矩阵（每个像素使用32位浮点数表示）
    cv::Mat mask = cv::Mat::ones(im.rows, im.cols, CV_32F);

    //4.构造一个大小等于输入图像大小的mask，并用1填充。对于每个输入的关键点，将其所在的单元格设置为“占用”状态，并将在其周围nhalfcell大小的区域内的掩码值设为0。
    for( const auto &px : vcurkps ) {
        //对于每个关键点，代码会将其所在的位置对应的voccupcells二维数组中的元素标记为true，表示该位置已被占用
        voccupcells[px.y / ncellsize][px.x / ncellsize] = true;
        //调用了cv::circle函数，将mask矩阵中以当前关键点位置px为中心、半径为nhalfcell的圆形区域填充为0（黑色），即在mask矩阵中将该区域对应的像素值设为0
        cv::circle(mask, px, nhalfcell, cv::Scalar(0.), -1);
    }

    // std::cout << "\n Single Scale detection \n";
    // std::cout << "\n nhcells : " << nhcells << " / nwcells : " << nwcells;
    // std::cout << " / nbcells : " << nhcells * nwcells;
    // std::cout << "\n cellsize : " << ncellsize;

    /*
    并行地处理每个单元格。对于每个单元格，通过高斯滤波和min-eigen值方法检测角点，并使用掩码进行过滤和约束，检查检测到的角点是否在指定的检测区域roi内，
    如果是，则将其添加到向量中。同时，如果检测到的角点值超过了一个阈值，则将其添加到向量vvdetectedpx或vvsecdetectionspx中。
    */
    size_t nboccup = 0;

    std::vector<std::vector<cv::Point2f>> vvdetectedpx(nbcells);

    std::vector<std::vector<cv::Point2f>> vvsecdetectionspx(nbcells);

    auto cvrange = cv::Range(0, nbcells);

    //使用 OpenCV 的 parallel_for_ 函数并行地遍历一个整数范围 cvrange，遍历过程中使用给定的 lambda 函数进行计算。
    parallel_for_(cvrange, [&](const cv::Range& range) {
        for( int i = range.start ; i < range.end ; i++ ) {

        //1.通过整数 i 计算对应的行 r 和列 c
        size_t r = floor(i / nwcells);
        size_t c = i % nwcells;

        //2.如果 voccupcells[r][c] 为真，表示该单元格已经被占据，因此跳过此次循环，继续处理下一个单元格
        if( voccupcells[r][c] ) {
                nboccup++;
                continue;
        }

        //3.计算单元格左上角像素的坐标 (x,y)
        size_t x = c*ncellsize;
        size_t y = r*ncellsize;
        
        //4.根据 (x,y) 和 ncellsize 创建一个矩形区域 hroi
        cv::Rect hroi(x,y,ncellsize,ncellsize);

        if( x+ncellsize < ncols-1 && y+ncellsize < nrows-1 ) {//5.如果 hroi 的右下角像素位置仍在原始图像的范围内，执行以下操作：
            cv::Mat hmap;
            cv::Mat filtered_im;
            cv::GaussianBlur(im(hroi), filtered_im, cv::Size(3,3), 0.);//a. 从原始图像中提取 hroi 区域的子图像，并使用高斯滤波器进行平滑处理
            cv::cornerMinEigenVal(filtered_im, hmap, 3, 3);//b. 计算 hmap，即子图像中每个像素点的最小特征值，作为角点响应值

            double dminval, dmaxval;
            cv::Point minpx, maxpx;

            cv::minMaxLoc(hmap.mul(mask(hroi)), &dminval, &dmaxval, &minpx, &maxpx);//c. 找到 hmap 中的最大值 dmaxval 和对应的像素点 maxpx
            maxpx.x += x;//d. 将 maxpx 的坐标转换到原始图像中的坐标。
            maxpx.y += y;

            if( maxpx.x < roi.x || maxpx.y < roi.y //e. 如果 maxpx 不在给定的感兴趣区域 roi 内，则跳过此次循环，继续处理下一个单元格。
                || maxpx.x >= roi.x+roi.width 
                || maxpx.y >= roi.y+roi.height )
            {
                continue;
            }

            if( dmaxval >= dmaxquality_ ) {//f. 如果 dmaxval 大于或等于 dmaxquality_，将 maxpx 添加到 vvdetectedpx[i] 中，并将 nhalfcell 半径内的像素点标记为已经使用过。
                vvdetectedpx.at(i).push_back(maxpx);
                cv::circle(mask, maxpx, nhalfcell, cv::Scalar(0.), -1);
            }

            cv::minMaxLoc(hmap.mul(mask(hroi)), &dminval, &dmaxval, &minpx, &maxpx);//g. 再次找到 hmap 中的最大值 dmaxval 和对应的像素点 maxpx
            maxpx.x += x;//h. 将 maxpx 的坐标转换到原始图像中的坐标。
            maxpx.y += y;

            if( maxpx.x < roi.x || maxpx.y < roi.y //i. 如果 maxpx 不在给定的感兴趣区域 roi 内，则跳过此次循环，继续处理下一个单元格。
                || maxpx.x >= roi.x+roi.width 
                || maxpx.y >= roi.y+roi.height )
            {
                continue;
            }

            if( dmaxval >= dmaxquality_ ) {//j. 如果 dmaxval 大于或等于 dmaxquality_，将 maxpx 添加到 vvsecdetectionspx[i] 中，并将 nhalfcell 半径内的像素点标记为已经使用过。
                vvsecdetectionspx.at(i).push_back(maxpx);
                cv::circle(mask, maxpx, nhalfcell, cv::Scalar(0.), -1);
            }
        }
    }   //6.完成对当前单元格的处理，进入下一个单元格的处理。
    });

    for( const auto &vpx : vvdetectedpx ) {  //对向量“vvdetectedpx”中的每个元素“vpx”进行迭代
        if( !vpx.empty() ) {
            vdetectedpx.insert(vdetectedpx.end(), vpx.begin(), vpx.end());//如果元素“vpx”不为空，则将其插入到向量“vdetectedpx”末尾
        }
    }

    size_t nbkps = vdetectedpx.size();//计算向量“vdetectedpx”的大小并将其保存在变量“nbkps”中

    if( nbkps+nboccup < nbcells ) {
        size_t nbsec = nbcells - (nbkps+nboccup);//如果“nbkps + nboccup”小于“nbcells”，则计算差值并将其保存在变量“nbsec”中
        size_t k = 0;
        for( const auto &vseckp : vvsecdetectionspx ) {//代码对向量“vvsecdetectionspx”中的每个元素“vseckp”进行迭代
            if( !vseckp.empty() ) {
                vdetectedpx.push_back(vseckp.back());//如果元素“vseckp”不为空，则将其最后一个元素插入到向量“vdetectedpx”中
                k++;
                if( k == nbsec ) {
                    break;//如果插入的元素数量达到了“nbsec”，则退出循环
                }
            }
        }
    }

    nbkps = vdetectedpx.size();//将vdetectedpx的大小赋值给nbkps变量

    if( nbkps < 0.33 * (nbcells - nboccup) ) {//检查nbkps是否小于0.33乘以（nbcells-nboccup）的结果
        dmaxquality_ /= 2.;//如果成立，则将dmaxquality_的值除以2。这表示，如果检测到的关键点数量小于可用单元格数量的33％，则会降低dmaxquality_的值。
    } 
    //如果第一个条件不成立，则检查nbkps是否大于0.9乘以（nbcells-nboccup）的结果。
    //如果成立，则将dmaxquality_的值乘以1.5。这表示，如果检测到的关键点数量大于可用单元格数量的90％，则会增加dmaxquality_的值。
    else if( nbkps > 0.9 * (nbcells - nboccup) ) {
        dmaxquality_ *= 1.5;
    }

    // Compute Corners with Sub-Pixel Accuracy以亚像素精度计算角点
    if( !vdetectedpx.empty() )//检查向量 vdetectedpx 是否为空
    {
        //向量不为空，则使用OpenCV库中的 cornerSubPix 函数查找和精细化输入图像 im 中检测到的角点
        /// Set the need parameters to find the refined corners
        /*
        winSize：窗口大小，用于计算角点的亚像素精度，这里设置为 3x3 的窗口大小。
        zeroZone：死区的大小，在死区内的角点不会被计算。这里将其设置为 -1x-1，表示不使用死区。
        criteria：计算角点位置的终止条件，它是一个 TermCriteria 类型的变量，包含三个参数。
        第一个参数 cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER 表示迭代次数和精度的终止条件；
        第二个参数 30 表示最大迭代次数；
        第三个参数 0.01 表示精度的阈值。
        */
        cv::Size winSize = cv::Size(3,3);
        cv::Size zeroZone = cv::Size(-1,-1);
        cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::EPS + 
                                        cv::TermCriteria::MAX_ITER, 30, 0.01);

        cv::cornerSubPix(im, vdetectedpx, winSize, zeroZone, criteria); //调用cornerSubPix函数来执行角点检测和细化操作，并返回检测到的角点位置。
    }

    // std::cout << "\n \t>>> Found : " << nbkps;

    return vdetectedpx;
}

/*
* 在输入图像im中使用FAST算法检测网格状的角点，并返回检测到的角点位置
函数的输入参数包括一个整数ncellsize表示每个网格单元的大小，
一个std::vector<cv::Point2f>类型的变量vcurkps表示之前检测到的角点位置，以及一个矩形区域roi表示感兴趣的图像区域。
*/
std::vector<cv::Point2f> FeatureExtractor::detectGridFAST(const cv::Mat &im, const int ncellsize, 
        const std::vector<cv::Point2f> &vcurkps, const cv::Rect &roi)
{   
    //对输入图像进行了检查，如果为空，则直接返回一个空的std::vector<cv::Point2f>类型的变量
    if( im.empty() ) {
        // std::cerr << "\n No image provided to detectGridFAST() !\n";
        return std::vector<cv::Point2f>();
    }

    size_t ncols = im.cols;
    size_t nrows = im.rows;

    size_t nhalfcell = ncellsize / 4;

    size_t nhcells = nrows / ncellsize;
    size_t nwcells = ncols / ncellsize;

    size_t nbcells = nhcells * nwcells;

    /*
    计算出网格的行数、列数以及网格总数，并定义一个vdetectedpx变量表示检测到的角点位置，
    它是一个std::vector<cv::Point2f>类型的变量，初始时预留了nbcells个元素的空间。
    */
    std::vector<cv::Point2f> vdetectedpx;
    vdetectedpx.reserve(nbcells);

    // 定义一个二维布尔型向量voccupcells表示每个网格单元是否被占用，初始化为全部为false
    std::vector<std::vector<bool>> voccupcells(
            nhcells+1, 
            std::vector<bool>(nwcells+1, false)
            );

    // 定义一个mask变量表示图像掩膜，用于将检测到的角点排除在网格单元的中心区域内
    cv::Mat mask = cv::Mat::ones(im.rows, im.cols, CV_32F);

    //使用vcurkps中的角点位置更新voccupcells，将网格单元标记为已占用，同时使用cv::circle函数在mask上绘制圆形区域，将检测到的角点排除在其中
    for( const auto &px : vcurkps ) {
        voccupcells[px.y / ncellsize][px.x / ncellsize] = true;
        cv::circle(mask, px, nhalfcell, cv::Scalar(0), -1);
    }

    //结束了对输入图像的预处理，后面将继续使用FAST算法检测网格状的角点

    //将nboccup和nbempty两个变量初始化为0，它们用来统计网格中的被占用和空闲单元格数量
    size_t nboccup = 0;
    size_t nbempty = 0;

    // Create the FAST detector if not set yet如果尚未设置，请创建 FAST 检测器
    //代码检查pfast_对象是否为空，如果是，则创建一个新的cv::FastFeatureDetector对象，阈值为nfast_th_
    if( pfast_ == nullptr ) {
        pfast_ = cv::FastFeatureDetector::create(nfast_th_);
    }

    // std::cout << "\ndetectGridFAST (cellsize: " << ncellsize << ") : \n";
    // std::cout << "\n FAST grid search over #" << nbcells;
    // std::cout << " cells (" << nwcells << ", " << nhcells << ")\n";
 
    //代码创建了一个大小为nbcells的std::vector对象，名为vvdetectedpx，用于存储每个单元格中检测到的特征点坐标
    std::vector<std::vector<cv::Point2f>> vvdetectedpx(nbcells);

    //parallel_for_函数，是一个并行处理的循环，它并行遍历每个单元格，执行循环中的代码块。cvrange表示遍历范围，它是一个包含了从0到nbcells-1的整数范围
    auto cvrange = cv::Range(0, nbcells);

    /*
    在循环中，变量i遍历了cvrange所表示的整数范围。变量r和c分别表示当前单元格的行和列坐标，它们的计算基于变量i和网格的列数nwcells。
    如果当前单元格被标记为被占用，则增加nboccup计数器并跳过该单元格的处理。否则，增加nbempty计数器并执行后面的代码。
    */
    //实现了在一个网格中使用FAST算法检测特征点，并统计了被占用和空闲单元格的数量
    parallel_for_(cvrange, [&](const cv::Range& range) {
        for( int i = range.start ; i < range.end ; i++ ) {

        size_t r = floor(i / nwcells);
        size_t c = i % nwcells;

        if( voccupcells[r][c] ) {
                nboccup++;
                continue;
        }

        nbempty++;

        //基于当前行和列索引（r和c）以及ncellsize参数在图像im中设置了一个名为hroi的矩形区域。
        //通过将行和列索引乘以ncellsize，计算出x和y值，这是预期分析的每个单元格或图像块的大小。
        size_t x = c*ncellsize;
        size_t y = r*ncellsize;
        
        cv::Rect hroi(x,y,ncellsize,ncellsize);//创建一个矩形hroi，左上角坐标为(x,y)，宽度和高度为ncellsize

        //如果矩形hroi的右下角在图像的尺寸范围内（由nrows和ncols给出），则使用pfast_检测器在矩形hroi内的图像im上检测关键点。掩码mask也在此区域应用。
        if( x+ncellsize < ncols-1 && y+ncellsize < nrows-1 ) 
        {

            std::vector<cv::KeyPoint> vkps;
            pfast_->detect(im(hroi), vkps, mask(hroi));

            //如果在hroi内没有检测到关键点，则继续循环到下一次迭代。
            if( vkps.empty() ) {
                continue;
            } else {//如果检测到关键点，则根据它们的响应值（一种衡量它们强度或显著性的度量）对它们进行排序。
                std::sort(vkps.begin(), vkps.end(), compare_response);
            }

            //如果最强关键点的响应值大于或等于20，则更新其位置到全局图像坐标，并在掩码上以此位置为中心绘制一个圆。该位置也附加到名为vvdetectedpx的向量中。
            if( vkps.at(0).response >= 20 ) {
                cv::Point2f pxpt = vkps.at(0).pt;
                
                pxpt.x += x;
                pxpt.y += y;

                cv::circle(mask, pxpt, nhalfcell, cv::Scalar(0), -1);

                vvdetectedpx.at(i).push_back(pxpt);
            }

        }
    }
    });

    //循环遍历一个检测到的关键点向量的向量 vvdetectedpx，并将所有非空的关键点向量添加到一个新向量 vdetectedpx 中
    for( const auto &vpx : vvdetectedpx ) {
        if( !vpx.empty() ) {
            vdetectedpx.insert(vdetectedpx.end(), vpx.begin(), vpx.end());
        }
    }

    //获取 vdetectedpx 向量的大小，计算检测到的关键点总数 nbkps
    size_t nbkps = vdetectedpx.size();

    // Update FAST th.
    // int nfast_th = pfast_->getThreshold();//更新用于在图像中检测关键点的FAST（从加速段测试得到的特征）算法的阈值

    /*
    如果检测到的关键点数量小于空关键点数量的一半（其中空关键点定义为没有检测到关键点的向量），
    并且有超过10个空关键点，则FAST算法的阈值将减少34％（乘以0.66）。
    */
    if( nbkps < 0.5 * nbempty && nbempty > 10 ) {
        nfast_th_ *= 0.66;
        pfast_->setThreshold(nfast_th_);//使用指向FAST算法类实例的指针（pfast_）来更新其阈值
    } 
    //如果检测到的关键点数量等于空关键点数量，则FAST算法的阈值将增加50％（乘以1.5）
    else if ( nbkps == nbempty ) {
        nfast_th_ *= 1.5;
        pfast_->setThreshold(nfast_th_);
    }

    // Compute Corners with Sub-Pixel Accuracy
    //对检测到的关键点进行精确化处理

    if( !vdetectedpx.empty() )//检查 vdetectedpx 向量是否为空，如果不为空，则执行下面的操作
    {
        /// Set the need parameters to find the refined corners
        /*
        设置了一些参数，以便找到精确化的角点。
        这些参数包括窗口大小 winSize、零区域 zeroZone 和终止准则 criteria。
        终止准则是当最大迭代次数或者满足指定精度时，停止迭代的条件。
        */
        cv::Size winSize = cv::Size(3,3);
        cv::Size zeroZone = cv::Size(-1,-1);
        cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::EPS + 
                                        cv::TermCriteria::MAX_ITER, 30, 0.01);

        //使用 cv::cornerSubPix 函数对图像 im 中的检测到的关键点 vdetectedpx 进行亚像素级别的精确化处理
        cv::cornerSubPix(im, vdetectedpx, winSize, zeroZone, criteria);
    }

    // std::cout << "\n \t>>> Found : " << vdetectedpx.size();//输出精确化后的关键点数量 vdetectedpx.size()
    return vdetectedpx;
}

/*
这段代码实现了一个特征提取器的函数 setMask，该函数用于生成一个掩码（mask），以便在图像处理过程中只处理图像中的特定区域。

函数接受三个输入参数和一个输出参数：

    im：一个输入图像，类型为 cv::Mat。
    vpts：一个由 cv::Point2f 构成的点向量，表示要生成掩码的圆心坐标。
    dist：一个整数，表示掩码圆的半径。
    mask：一个输出参数，类型为 cv::Mat，表示生成的掩码图像。

1.函数首先检查 mask 是否为空。如果为空，则使用 cv::Mat 构造函数生成一个与 im 大小相同、类型为 CV_8UC1（即单通道 8 位无符号整数）的图像，
并将所有像素的值设置为 255（即白色）。如果 mask 已经包含了一些像素值，则不会进行覆盖。

2.然后，函数对 vpts 中的每个点执行以下操作：
    调用 cv::circle 函数，在 mask 中绘制一个以该点为圆心、半径为 dist 的黑色圆形。这个函数的最后一个参数 -1 表示将圆形填充为黑色。

执行完这些操作后，函数返回 mask，其中包含了一个掩码图像，用于在图像处理中选择特定区域进行操作。
*/
void FeatureExtractor::setMask(const cv::Mat &im, const std::vector<cv::Point2f> &vpts, const int dist, cv::Mat &mask) const
{
    if( mask.empty() ) {
        mask = cv::Mat(im.rows, im.cols, CV_8UC1, cv::Scalar(255));
    }

    for (auto &pt : vpts) {
        cv::circle(mask, pt, dist, 0, -1);
    }
}