slam14book2的ch7中3

slam14book2的ch7中3

使用了vins-mono和orbslam2的思想去实现了两个特征点均匀化的代码

对于四叉树可以看下leetcode的这道题目：
427、建立四叉树
不过题解中大多都用dfs去搜索的，但是对于orb均匀化的操作最好还是像orb作者一样用queue这种进行bfs

// 特征点均匀化方法
// 

#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;


struct node{
    int left;
    int right;
    int up;
    int down;
    vector<int> featureIndex;// 特征点的索引
};
// 父节点 总共的特征点 
// 这个不能用递归去做，要用队列去做---为了保证是平行调用的
void divisionNode(node &father, std::vector<KeyPoint>& keypoints, int targetNum, vector<KeyPoint>& rekeypoints){
    queue<node *> nodeQue;
    nodeQue.push(&father);// 把根结点入队
    int sizet = 0;
    cout << keypoints.size() << endl;
    while(1){
        // 分为上下左右四个区域
        node* nowmakenode = nodeQue.front();
        nodeQue.pop();
        // 新建node节点
        node* leftup = new node();
        node* rightup = new node();
        node* leftdown = new node();
        node* rightdown = new node();
        // 把father node的特征点给分了
        int h_middle = (nowmakenode->up + nowmakenode->down)/2;// 240
        int l_middle = (nowmakenode->left + nowmakenode->right)/2;// 320
        leftup->left = nowmakenode->left;
        leftup->up = nowmakenode->up;
        leftup->right = l_middle;
        leftup->down = h_middle;

        rightup->left = l_middle;
        rightup->up = nowmakenode->up;
        rightup->right = nowmakenode->right;
        rightup->down = h_middle;

        leftdown->left = nowmakenode->left;
        leftdown->up = h_middle;
        leftdown->right = l_middle;
        leftdown->down = nowmakenode->down;

        rightdown->left = l_middle;
        rightdown->up = h_middle;
        rightdown->right = nowmakenode->right;
        rightdown->down = nowmakenode->down;
        // 按照两个middle进行索引去
        for(int i = 0; i < nowmakenode->featureIndex.size(); i++){
            Point2f nowpoint = keypoints[nowmakenode->featureIndex[i]].pt;
            if(nowpoint.y < l_middle){// 在左边
                if(nowpoint.x < h_middle){// 在上面
                    leftup->featureIndex.push_back(nowmakenode->featureIndex[i]);
                }else{
                    leftdown->featureIndex.push_back(nowmakenode->featureIndex[i]);
                }
            }else{// 在右面
                if(nowpoint.x < h_middle){// 在上面
                    rightup->featureIndex.push_back(nowmakenode->featureIndex[i]);
                }else{
                    rightdown->featureIndex.push_back(nowmakenode->featureIndex[i]);
                }
            }
        }

        // 如果size == 1,直接把这个节点塞进去
        //          》 1加入到queue中
        //         == 0 直接free掉
        if(leftup->featureIndex.size() == 0) delete leftup;
        else if(leftup->featureIndex.size() == 1){
            sizet++;
            rekeypoints.push_back(keypoints[leftup->featureIndex[0]]);
            delete leftup;
        }else{
            nodeQue.push(leftup);
        }

        if(leftdown->featureIndex.size() == 0) delete leftdown;
        else if(leftdown->featureIndex.size() == 1){
            sizet++;
            rekeypoints.push_back(keypoints[leftdown->featureIndex[0]]);
            delete leftdown;
        }else{
            nodeQue.push(leftdown);
        }

        if(rightup->featureIndex.size() == 0) delete rightup;
        else if(rightup->featureIndex.size() == 1){
            sizet++;
            rekeypoints.push_back(keypoints[rightup->featureIndex[0]]);
            delete rightup;
        }else{
            nodeQue.push(rightup);
        }

        if(rightdown->featureIndex.size() == 0) delete rightdown;
        else if(rightdown->featureIndex.size() == 1){
            sizet++;
            rekeypoints.push_back(keypoints[rightdown->featureIndex[0]]);
            delete rightdown;
        }else{
            nodeQue.push(rightdown);
        }
        cout << "nodeQue：" <<  nodeQue.size() << endl;
        // 判断下数量
        if(sizet + nodeQue.size() >= targetNum ){// 数量够了
            while(nodeQue.size()>0){
                // 简单的就只要第一个点
                node* makenode = nodeQue.front();
                nodeQue.pop();
                rekeypoints.push_back(keypoints[makenode->featureIndex[0]]);
                delete makenode;
            }
            break;
        }
       
        if(nodeQue.size() == 0){//所有节点都不能分裂了
            break;
        }
        //break;

    }

} 

int main(char argc, char **argv){
    // read picture
    Mat img= imread("1.png");
	if (!img.data)
	{
		cout << "error reading images " << endl;
		return -1;
	}
    Mat gry;
    cvtColor(img, gry, COLOR_BGR2GRAY);
    std::vector<KeyPoint> keypoints;//关键点容器  二维点
    Mat descriptors;			      //关键点对应的描述子
    Ptr<FeatureDetector> detector = ORB::create();  //cv3下　ORB特征检测    其他 BRISK   FREAK
    Ptr<DescriptorExtractor> descriptor = ORB::create();//cv3下　ORB描述子
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );
    detector->detect (gry,keypoints );
    Mat showImage1;
    drawKeypoints( img, keypoints, showImage1, Scalar::all(-1), DrawMatchesFlags::DEFAULT );
    // 原图展示
    imshow("picture",showImage1);
    
    // 使用vins mono的方法进行极大值抑制的方法
    // 光流追踪的上一帧数据进行操作，按照光流追踪的评分进行排序
    // 按照排序的情况放进图片中，如果那个位置被mask了就跳过
    // 如果没有被mask过，就给他加入到集合中，然后在mask图片上这个特征点周围画一个圆

    // 依照此策略直接开干（假设特征点已经按照评分进行排序了）
    vector<KeyPoint> resiveID;
    Mat mask = cv::Mat(img.rows, img.cols, CV_8UC1, cv::Scalar(255));
    int MIN_DIST = 10;// 10个像素之内不再接受这个
    for(int i = 0; i < keypoints.size(); i++){
        Point2f pointLocation = keypoints[i].pt;// 点
        if (mask.at<uchar>(pointLocation) == 255){ // 是否被占用
            resiveID.push_back(keypoints[i]);// 
            cv::circle(mask, pointLocation, MIN_DIST, 0, -1);
        }
    }
    // show
    Mat showImage2;
    drawKeypoints( img, resiveID, showImage2, Scalar::all(-1), DrawMatchesFlags::DEFAULT );
    // vins策略展示
    imshow("picture2",showImage2);


    // 再使用orbslam2的策略
    // orbslam2使用了一个四叉树的结构，只要分裂个数没有到达想要留下特征点的个数，就会接着分裂
    vector<KeyPoint> resivepoint2;
    node father;
    father.left = 0;
    father.right = img.cols;
    father.up = 0;
    father.down = img.rows;
    for(int i = 0; i < keypoints.size(); i++) 
        father.featureIndex.push_back(i);
    divisionNode(father, keypoints, 30, resivepoint2);
    cout << "orb:" << resivepoint2.size() << endl; 
    Mat showImage3;
   
    drawKeypoints( img, resivepoint2, showImage3, Scalar::all(-1), DrawMatchesFlags::DEFAULT );
    imshow("picture3",showImage3);

   
    waitKey(0);

}

实验效果记录：

原图

vins-mono思想

orb-slam2思想