ORB特征四叉树均匀分布算法的实现

本文主要论述了在ORB-SLAM2中的图像预处理，均匀化特征点分布的过程
主要参考了以下文章。

https://blog.csdn.net/qq_17032807/article/details/84994607
https://blog.csdn.net/qq_30356613/article/details/75231440
https://blog.csdn.net/weixin_38636815/article/details/81981305
https://blog.csdn.net/weixin_38636815/article/details/81943109

以下为具体的代码实现：

#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace cv;

int features_num = 1000;
float scale_factor = 1.2f;
int levels_num = 8;
int default_fast_T = 20;   //FAST默认检测阈值
int min_fast_T = 7;        //FAST最小检测阈值
int edge_threshold = 19;   //边界尺度
int PATCH_SIZE = 31;

//定义四叉树节点类
class ExtractorNode{
public:
    ExtractorNode():bNoMore(false){}

    void DivideNode(ExtractorNode& n1,ExtractorNode& n2,ExtractorNode& n3,ExtractorNode& n4);
    std::vector vkeys;
    cv::Point2i UL,UR,BL,BR;
    std::list::iterator lit;
    bool bNoMore;   //用来判定是否继续分割

};


//四叉树节点划分函数
void ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4)
{
	/*四叉树四个节点示意图
   * 
   * ---UL------------------------------------------------UR----------------
   *	/                          /                      /
   *	/                          /				      /
   *	/                          /				      /
   *	/                          /				      /
   *	/             n1           /		n2		      /
   *	/                          /				      /
   *	/                          /				      /
   *	/                          /				      /
   *	/                          / 				      /
   *	/--------------------------/------------------------------------	 
   *	/                          / 				      /
   *	/                          / 				      /
   *	/                          / 				      /
   *	/               n3         / 		n4		      /
   *	/                          / 				      /
   *	/                          / 				      /
   *	/                          / 				      /
   *	/                          / 				      /
   *	/                          /				      /
   *----BL------------------------------------------------BR-----------------
   * 
   */

    const int halfx = ceil(static_cast(UR.x - UL.x)/2);
    const int halfy = ceil(static_cast(BR.y - UL.y)/2);

    n1.UL = UL;
    n1.UR = cv::Point2i(UL.x+halfx,UL.y);
    n1.BL = cv::Point2i(UL.x,UL.y+halfy);
    n1.BR = cv::Point2i(UL.x+halfx,UL.y+halfy);
    n1.vkeys.reserve(vkeys.size());

    n2.UL = n1.UR;
    n2.UR = UR;
    n2.BL = n1.BR;
    n2.BR = cv::Point2i(UR.x,UL.y+halfy);
    n2.vkeys.reserve(vkeys.size());

    n3.UL = n1.BL;
    n3.UR = n1.BR;
    n3.BL = BL;
    n3.BR = cv::Point2i(UL.x+halfx,BL.y);
    n3.vkeys.reserve(vkeys.size());

    n4.UL = n3.UR;
    n4.UR = n2.BR;
    n4.BL = n3.BR;
    n4.BR = BR;
    n4.vkeys.reserve(vkeys.size());

    for (size_t i = 0; i < vkeys.size(); ++i)
    {
        const cv::KeyPoint& kp = vkeys[i];
        if(kp.pt.x < n1.UR.x)
        {
            if(kp.pt.y  feature_num_per_level;
    vector vec_scale_per_factor;

    vec_scale_per_factor.resize(levels_num);
    vec_scale_per_factor[0] = 1.0f;
    for (int i = 1; i < levels_num; ++i) {
        vec_scale_per_factor[i] = vec_scale_per_factor[i-1]*scale_factor;
    }

    std::vector vec_img_pyramid(levels_num);
    for (int level = 0; level < levels_num; ++level) {
        float scale = 1.0f/vec_scale_per_factor[level];
        cv::Size sz(cvRound((float)img.cols*scale),cvRound((float)img.rows*scale));

        if (level == 0)
        {
            vec_img_pyramid[level] = img;
        } else
            resize(vec_img_pyramid[level-1],vec_img_pyramid[level],sz,0,0,CV_INTER_LINEAR);

        cout<<"正在构建第 "<> all_keypoints;
    all_keypoints.resize(levels_num);

    const float border_width = 30;

    for (int level = 0; level < levels_num; ++level)
    {
        const int min_boder_x = edge_threshold - 3;
        const int min_boder_y = min_boder_x;
        const int max_boder_x = vec_img_pyramid[level].cols - edge_threshold +3;
        const int max_boder_y = vec_img_pyramid[level].rows - edge_threshold +3;

        vector vec_to_per_distribute_keys;
        vec_to_per_distribute_keys.reserve(features_num*10);

        const float width = max_boder_x - min_boder_x;
        const float height = max_boder_y -min_boder_y;

        const int cols = width/border_width;
        const int rows = height/border_width;

        const int width_cell = ceil(width/cols);
        const int height_cell = ceil(height/rows);

        cout<<"第"<= max_boder_y-3)
                continue;
            if (max_y >= max_boder_y)
                max_y = max_boder_y;

            for (int j = 0; j < cols; ++j)
            {
                const float ini_x = min_boder_x + j*width_cell;
                float max_x = ini_x + width_cell + 6;

                if(ini_x >= max_boder_x-6)
                    continue;
                if (max_x >= max_boder_x)
                    max_x = max_boder_x;

                std::vector vec_keys_cell;
				
				//opencv自带的FAST角点检测函数
                cv::FAST(vec_img_pyramid[level].rowRange(ini_y,max_y).colRange(ini_x,max_x),vec_keys_cell,default_fast_T,
                         true);

                if (vec_keys_cell.empty())
                {
                    cv::FAST(vec_img_pyramid[level].rowRange(ini_y,max_y).colRange(ini_x,max_x),vec_keys_cell,min_fast_T,
                             true);
                }

                if (!vec_keys_cell.empty())
                {
                    for (std::vector::iterator vit = vec_keys_cell.begin(); vit != vec_keys_cell.end();vit++)
                    {
                        (*vit).pt.x += j*width_cell;
                        (*vit).pt.y += i*height_cell;
                        vec_to_per_distribute_keys.push_back(*vit);
                    }

                }

            }

        }

        cout<<"这层图像共有 "<& keypoints = all_keypoints[level];
        keypoints.reserve(features_num);
		
		//划分根节点
        const int init_node_num = round(static_cast(max_boder_x-min_boder_x)/(max_boder_y-min_boder_y));
        cout<<"初始化时有 "<(max_boder_x-min_boder_x)/init_node_num;
        cout<<"节点间隔： "< list_nodes;   //用来存储所有的四叉树节点
        std::vector init_nodes_address;   //用来存储所有的四叉树节点中的vkeys
        init_nodes_address.resize(init_node_num);
		
		
		//根节点的UL，UR，BL，BR是不含边界的坐标，在最后的关键点坐边要补加上边界大小
        for (int i = 0; i < init_node_num; ++i)
        {
            ExtractorNode ni;
            ni.UL = cv::Point2i(interval_x* static_cast(i),0);
            ni.UR = cv::Point2i(interval_x* static_cast(i+1),0);
            ni.BL = cv::Point2i(ni.UL.x,max_boder_y-min_boder_y);
            ni.BR = cv::Point2i(ni.UR.x,max_boder_y-min_boder_x);
            ni.vkeys.reserve(vec_to_per_distribute_keys.size());

            list_nodes.push_back(ni);
            init_nodes_address[i] = &list_nodes.back();
        }

        for (size_t i = 0; i < vec_to_per_distribute_keys.size(); ++i)
        {
            const cv::KeyPoint &kp = vec_to_per_distribute_keys[i];
            init_nodes_address[kp.pt.x/interval_x]->vkeys.push_back(kp);

        }
		
         //list_nodes中的结点和 init_nodes_address中的结点指针是同步的，只有在 list_nodes中存储的结点中存储了特征点，才能根据特征点的数目来决定如何处理这个结点
		 //那如果在list_nodes中删除一个没有特征点的结点，那么在 init_nodes_address中对应的这个地址也会被销毁
        list::iterator lit = list_nodes.begin();
        while (lit != list_nodes.end())
        {
            if (lit->vkeys.size() == 1)
            {
                lit->bNoMore = true;
                lit++;
            } else if (lit->vkeys.empty())
            {
                lit = list_nodes.erase(lit);
            } else
                lit++;
        }



		//开始划分四叉树节点
        bool is_finish = false;
        int iteration = 0;

        std::vector> key_size_and_node;
        key_size_and_node.reserve(list_nodes.size()*4);

        while (!is_finish)
        {
            iteration++;
            int pre_size = list_nodes.size();

            lit = list_nodes.begin();
            int to_expand_num = 0;
            key_size_and_node.clear();

            while (lit != list_nodes.end()) {
                if (lit->bNoMore) {
                    lit++;
                    continue;
                } else {
                    ExtractorNode n1, n2, n3, n4;
                    lit->DivideNode(n1, n2, n3, n4);

                    if (n1.vkeys.size() > 0) {
                        list_nodes.push_front(n1);
                        if (n1.vkeys.size() > 1) {
                            to_expand_num++;
                            key_size_and_node.push_back(std::make_pair(n1.vkeys.size(), &list_nodes.front()));
                            list_nodes.front().lit = list_nodes.begin();  //把节点中的迭代器指向自身，在后面的判断条件中使用
                        }
                    }
                    if (n2.vkeys.size() > 0) {
                        list_nodes.push_front(n2);
                        if (n2.vkeys.size() > 1) {
                            to_expand_num++;
                            key_size_and_node.push_back(std::make_pair(n2.vkeys.size(), &list_nodes.front()));
                            list_nodes.front().lit = list_nodes.begin();
                        }
                    }
                    if (n3.vkeys.size() > 0) {
                        list_nodes.push_front(n3);
                        if (n3.vkeys.size() > 1) {
                            to_expand_num++;
                            key_size_and_node.push_back(std::make_pair(n3.vkeys.size(), &list_nodes.front()));
                            list_nodes.front().lit = list_nodes.begin();
                        }
                    }
                    if (n4.vkeys.size() > 0) {
                        list_nodes.push_front(n4);
                        if (n4.vkeys.size() > 1) {
                            to_expand_num++;
                            key_size_and_node.push_back(std::make_pair(n4.vkeys.size(), &list_nodes.front()));
                            list_nodes.front().lit = list_nodes.begin();
                        }
                    }

                    lit = list_nodes.erase(lit);
                    continue;
                }

            }
			
			//计算当前level上应该分布多少特征点
			//等比数列sn = a1(1-q^level)/(1-q)
			//sn是总的特征数目，a1是level=0 层上的特征点数
		
            feature_num_per_level.resize(levels_num);
            float factor = 1.0f / scale_factor;
            float desired_feature_per_scale =
                    features_num * (1 - factor) / (1 - (float) pow((double) factor, (double) levels_num));
            int sum_features = 0;
            for (int i = 0; i < levels_num - 1; ++i) {
                feature_num_per_level[i] = cvRound(desired_feature_per_scale);
                sum_features += feature_num_per_level[i];
                desired_feature_per_scale *= factor;
            }
            feature_num_per_level[levels_num - 1] = std::max(features_num - sum_features, 0);

			//判断四叉树结束条件
			//当创建的结点的数目比要求的特征点还要多或者，每个结点中都只有一个特征点的时候就停止分割
            if ((int) list_nodes.size() >= features_num || (int) list_nodes.size() == pre_size) {
                is_finish = true;
				
            } //如果现在生成的结点全部进行分割后生成的结点满足大于需求的特征点的数目，但是不继续分割又不能满足大于N的要求时
			  //这里为什么是乘以三，这里也正好印证了上面所说的当一个结点被分割成四个新的结点时，
             //这个结点时要被删除的，其实总的结点时增加了三个
			else if (((int) list_nodes.size() + to_expand_num * 3) > feature_num_per_level[level]) {
                while (!is_finish) {
                    pre_size = list_nodes.size();
                    std::vector > prve_size_and_node_adderss = key_size_and_node;
                    key_size_and_node.clear();

                    sort(prve_size_and_node_adderss.begin(), prve_size_and_node_adderss.end());

                    for (int j = prve_size_and_node_adderss.size() - 1; j >= 0; --j) {
                        ExtractorNode n1, n2, n3, n4;
                        prve_size_and_node_adderss[j].second->DivideNode(n1, n2, n3, n4);

                        if (n1.vkeys.size() > 0) {
                            list_nodes.push_front(n1);
                            if (n1.vkeys.size() > 1) {
                                key_size_and_node.push_back(std::make_pair(n1.vkeys.size(), &list_nodes.front()));
                                list_nodes.front().lit = list_nodes.begin();
                            }
                        }

                        if (n2.vkeys.size() > 0) {
                            list_nodes.push_front(n2);
                            if (n2.vkeys.size() > 1) {
                                key_size_and_node.push_back(std::make_pair(n2.vkeys.size(), &list_nodes.front()));
                                list_nodes.front().lit = list_nodes.begin();
                            }
                        }
                        if (n3.vkeys.size() > 0) {
                            list_nodes.push_front(n3);
                            if (n3.vkeys.size() > 1) {
                                key_size_and_node.push_back(std::make_pair(n3.vkeys.size(), &list_nodes.front()));
                                list_nodes.front().lit = list_nodes.begin();
                            }
                        }
                        if (n4.vkeys.size() > 0) {
                            list_nodes.push_front(n4);
                            if (n4.vkeys.size() > 1) {
                                key_size_and_node.push_back(std::make_pair(n4.vkeys.size(), &list_nodes.front()));
                                list_nodes.front().lit = list_nodes.begin();
                            }
                        }

                        list_nodes.erase(prve_size_and_node_adderss[j].second->lit);
                        if ((int) list_nodes.size() >= feature_num_per_level[level]) {
                            break;
                        }
                    }

                    if ((int) list_nodes.size() >= features_num || (int) list_nodes.size() == pre_size)
                        is_finish = true;
                }

            }
        }
				
		//四叉树划分完成，在每个节点中留下一个keypoints
        std::vector result_keys;
        result_keys.reserve(feature_num_per_level[level]);

        for (std::list::iterator lit = list_nodes.begin();lit != list_nodes.end();lit++)
        {
            vector& node_keys = lit->vkeys;
            cv::KeyPoint* keypoint = &node_keys[0];
            float max_response = keypoint->response;

            for (size_t k = 1; k < node_keys.size(); ++k)
            {
                if (node_keys[k].response > max_response)
                    {
                        keypoint = &node_keys[k];
                        max_response = node_keys[k].response;
                    }
                }

                result_keys.push_back(*keypoint);

            }

            keypoints = result_keys;
			
			
			//添加边界尺寸，计算特征点Patch的大小，根据每层的尺度的不同而不同
            const int scale_patch_size = PATCH_SIZE*vec_scale_per_factor[level];

            const int kps = keypoints.size();
            for (int l = 0; l < kps; ++l)
            {
                keypoints[l].pt.x += min_boder_x;
                keypoints[l].pt.y += min_boder_y;
                keypoints[l].octave = level;           //特征点所在图像金字塔的层
                keypoints[l].size = scale_patch_size; //特征点邻域直径，因为最初的网格大小是30，所以乘上比例系数
            }
            cout<<"经过四叉数筛选，第 "< out_put_all_keypoints(num_keypoints);

    for (int level = 0; level < levels_num; ++level)
    {
        if (level == 0 )
        {
            for (int i = 0; i < all_keypoints[level].size(); ++i)
            {
                out_put_all_keypoints.push_back(all_keypoints[level][i]);
            }
        }

        float scale = vec_scale_per_factor[level];
        for (vector::iterator key = all_keypoints[level].begin();key != all_keypoints[level].end();key++)
        {
            key->pt *= scale; //尺度恢复
        }

        out_put_all_keypoints.insert(out_put_all_keypoints.end(),all_keypoints[level].begin(),all_keypoints[level].end());
    }
	
	//对比试验
    cv::Mat out_img1;
    cv::drawKeypoints(img,out_put_all_keypoints,out_img1);
    cv::imshow("四叉数",out_img1);
    waitKey(0);

  	cv::Mat img2;
	vector fast_keypoints;
	 Ptr orb = ORB::create(1000);
	orb->detect(img,fast_keypoints);
	 drawKeypoints(img,fast_keypoints,img2);
	cv::imshow("orb",img2);
	cv::waitKey(0);

    return 0;
}

效果图如下：