slam14book2的ch7中3
使用了vins-mono和orbslam2的思想去实现了两个特征点均匀化的代码
// 特征点均匀化方法
//
#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思想
