forked from urbste/MultiCol-SLAM
-
Notifications
You must be signed in to change notification settings - Fork 0
/
mdBRIEFextractor.h
142 lines (114 loc) · 11 KB
/
mdBRIEFextractor.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
/**
* This file is part of ORB-SLAM.
*
* Copyright (C) 2014 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <http://webdiis.unizar.es/~raulmur/orbslam/>
*
* ORB-SLAM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef MDBRIEFEXTRACTOR_H
#define MDBRIEFEXTRACTOR_H
#include <vector>
#include <list>
#include <opencv/cv.h>
#include <opencv2/opencv.hpp>
#include "BaseComponents/slam_multi_cam_fisheye/multi_cam_model_omni.h"
static int learned_orb_pattern_64_fisheyeOCAM_corr2[4 * 512] =
{
-14, -14, -8, 4, -14, 2, -14, 9, -1, -11, 0, 14, 6, 2, 8, 15, 7, 8, 8, -11, 2, -9, 3, -14, -14, 12, -11, 10, 12, -10, 15, -12, -5, 10, -5, 14, 12, 13, 15, 15, -7, 3, -6, 0, -13, -11, -12, -14, -14, -14, -11, -13, 9, -13, 12, -11, 14, 1, 15, 4, -12, 8, -9, 11, -4, -2, -2, 1, 7, 15, 10, 14, -7, -13, -4, -14, 4, 2, 7, 4, -2, -10, -1, -3, 13, 5, 15, 2, 11, -2, 13, -5, 14, -14, 14, -7, -4, 9, -3, 5, 13, -4, 15, -1, 0, 0, 1, 4, 7, 12, 9, 15, 1, 3, 2, 0, 2, 15, 3, 12, -14, -2, -12, -5, -5, -14, -4, -11, 11, 5, 14, 6, -11, -1, -8, 0, -5, -2, -4, -5, -8, 10, -5, 8, 7, 7, 9, 4, 5, -5, 7, -2, 5, -13, 8, -14, 14, 7, 15, 10, 8, -1, 11, -2, 0, -14, 3, -13, -6, 13, -3, 15, -7, 0, -4, -1, 2, 0, 5, -4, -1, 15, 3, 15, -9, 15, -8, -10, 14, 14, 15, -10, -14, 4, -13, -3, 6, -4, 7, -8, -11, 6, -9, 3, 7, -2, 8, 1, 13, 15, 14, 12, 7, 1, 8, 5, -14, 8, -14, 13, 2, 12, 3, 15, -9, 2, -8, 5, -14, -6, -11, -4, 8, -8, 10, -11, -14, 1, -12, 4, -6, -4, -5, -1, 13, 10, 14, 7, 1, 9, 2, 12, -7, -9, -5, -6, -11, -14, -8, -11, -12, -5, -11, -8, 12, -7, 13, -4, 4, -11, 5, -8, 2, 3, 2, 7, -7, -11, -6, -14, -6, 7, -5, 10, 11, 11, 14, 10, -10, 12, -9, 9, 11, -4, 15, -4, -12, -7, -9, -8, 9, 1, 12, 3, 14, -5, 15, -8, -14, -13, -13, -9, 2, 11, 4, 7, -11, 15, -7, 15, 3, -11, 9, 15, 0, -7, 2, -10, 6, 8, 9, 10, 14, -11, 15, -14, -14, -8, -4, 12, -1, 0, 2, 1, -3, -2, 2, 15, 15, 10, 15, 14, 4, 8, 7, 7, -4, 11, 1, -5, -12, 6, -5, -14, 3, 15, 12, -12, 1, -7, 10, -6, 3, -14, 3, 4, 8, -1, 9, -4, 14, -13, 14, 6, 4, -5, 5, 9, 11, 0, 15, 12, -5, -11, -3, 8, -6, 1, -6, 8, -2, -12, -1, -8, 9, 8, 11, 11, 13, 3, 15, -3, -10, 11, -10, 15, -14, 8, -11, 5, 10, -5, 12, 8, -3, 12, -2, 9, -7, -2, -6, 3, -9, 15, -7, 12, -10, 0, -9, -3, -9, 7, -9, 11, 9, 2, 10, -1, 9, 5, 10, 2, 0, -6, 1, 5, 9, -9, 10, -6, -11, 5, -10, 8, -13, -3, -11, 0, 3, 6, 4, 9, -9, -5, -7, -2, 12, 9, 15, 10, -4, 15, -3, 12, -14, 3, -11, 2, 8, -14, 8, -10, -1, -5, 0, -2, 8, 15, 9, 12, -3, 6, -2, 3, 9, 11, 10, 8, 8, 4, 9, 7, 4, -7, 5, -4, 11, 11, 13, 14, 6, 3, 7, 0, 12, 0, 15, 1, -14, 6, -11, 7, -9, -8, -8, -11, 0, 6, 0, 10, 12, -9, 15, -8, -2, 12, -1, 15, 6, 10, 7, 13, 6, -5, 6, 0, 4, 8, 5, 5, -5, 1, -4, -2, 1, -4, 2, -7, -8, 9, -7, 6, -11, 10, -8, 9, -4, -7, -3, -10, 11, 3, 14, 2, 9, -10, 12, -11, -12, 1, -9, -1, -14, 14, -11, 15, -3, 3, -2, 6, -8, -11, -7, -8, 9, 13, 12, 14, -10, -7, -7, -6, -10, 5, -7, 4, 2, 11, 4, -12, -10, 15, -4, -2, -1, 0, 0, -3, -12, -13, -9, -14, 9, -11, 10, -14, -14, -7, -13, -3, -6, -10, -6, -1, -9, -2, -7, -5, 9, -3, 12, -1, -9, 12, -7, 15, 8, -5, 11, -7, 7, 0, 15, -14, -14, 15, -13, 11, -7, 3, -5, 6, -10, -5, -10, -1, -9, 0, -6, 2, 2, -14, 4, -10, -7, 15, -4, 14, -7, -12, -4, -10, 8, -7, 11, -6, -7, 6, -4, 3, 2, -11, 5, -14, 9, 13, 14, -2, 8, 5, 11, 4, 4, 13, 7, 15, 7, -9, 12, 3, -9, 8, -6, 9, 10, 6, 10, 10, -9, -9, -6, -10, -5, -7, -2, -4, -2, -14, 3, 10, -6, -5, -4, -8, -4, 14, -1, 12, 2, -3, 5, 3, -3, 9, -1, 12, -11, -10, -11, -6, 3, 15, 6, 14, -3, -13, 0, -14, 3, -13, 6, -12, 0, 11, 3, 14, 7, 5, 15, -6, 4, -3, 12, 12, 6, 4, 9, 5, 6, -2, 9, -1, 0, -8, 3, -5, -10, -14, 0, 5, -3, 9, 0, 7, -5, 8, -2, 9, -4, 7, 1, -13, -4, -7, -1, -8, -3, 2, 0, 0, 1, 7, 4, 8, 5, 12, 14, 6, -12, 3, -1, 15, -14, -1, -2, -8, 0, -12, 15, 10, -14, 15, 3, -1, -13, -14, 4, -11, -14, -9, -11, 9, -13, -13, -9, -6, -10, 4, -9, -1, 1, -5, 2, 15, -12, 2, -11, 6, -7, 13, -5, 8, -2, -3, -2, 10, 14, 8, 15, -1, -3, -7, -2, 2, 4, -6, 5, -11, 9, -5, 11, -9, -14, 15, -9, 3, 8, -10, 10, 13, 8, -10, 8, 1, -14, 7, -13, 10, -14, 14, -10, -4, 12, 12, 13, 9, 8, -12, 8, -5, 12, -6, 15, -9, -5, 15, -4, -7, -14, -4, -13, -7, 9, -6, 10, -3, -14, -11, -11, -9, -4, 6, -4, 11, 10, 2, 11, -5, 11, 8, 12, 4, -10, -5, -8, 3, 11, 1, 13, 5, 8, -4, 9, -1, -6, 5, -5, 2, -1, 6, -1, 15, 11, -13, 12, -10, -2, -14, -1, -11, -7, 5, -7, 9, 10, 5, 12, 8, -14, 11, -13, 14, 11, 6, 15, 14, -3, -6, -2, -3, -6, -14, -4, -5, 5, 1, 6, 9, 11, 4, 12, 0, 11, -1, 13, 2, -14, 5, -12, 1, -8, -13, -6, -10, -9, -5, -8, 9, 4, 8, 5, 11, -10, -7, -8, -4, 12, -13, 15, -14, 10, -9, 13, -13, 12, 1, 15, -10, -1, 15, 0, 12, -2, 3, -2, 9, -2, 10, -1, 7, -10, -14, -6, 11, 8, -1, 9, 3, -4, -4, -3, 5, 1, -2, 2, 1, 6, 14, 7, -14, -10, 10, -8, 7, -10, 0, -9, 4, 6, -10, 8, -14, 0, -10, 1, -7, -14, -8, -13, -11, 13, -10, 15, -7, 3, -5, 4, -8, -11, 9, -10, 12, 12, 3, 15, 6, -6, 7, -5, 4, -14, 9, -11, 8, -13, 2, -11, -1, -4, -9, -3, -6, 11, -5, 12, -1, -14, 4, -12, 7, -14, -2, -11, -1, 7, -7, 8, -4, 8, -3, 9, -6, 5, -1, 6, 2, 6, 6, 7, 9, -6, 1, -5, 4, -12, 15, -9, 13, 5, -9, 6, -12, 5, -2, 6, -6, 1, -7, 2, -4, 4, -9, 5, 15, -11, -2, -10, -6, -9, 5, -7, 2, 7, 1, 8, -3, 3, -5, 4, -1, -9, 3, -9, 7, -1, -11, 0, -14, -13, 6, -10, 5, 0, 12, 1, 9, 14, -5, 15, 3, -4, 4, -3, 1, 4, 1, 5, 4, -2, -1, -1, 2, -14, 14, -12, -14, 4, 4, 5, -5, 11, -8, 14, -9, -6, 10, -4, 7, -6, -6, -5, -3, 15, -10, 15, -4, -11, -7, -10, -10, -1, -9, 0, -12, 1, 7, 2, -1, 12, -1, 15, -2, -10, -11, -9, -14, -14, -3, -11, -4, -9, 10, -8, 13, 10, -13, 13, -14, 8, -3, 9, 12, 2, -10, 2, -5, 2, -14, 4, 14, 11, 7, 14, 9, -12, 14, -10, 10, -9, -4, -8, -7, -14, -1, -13, 3, -11, -10, -8, -8, -13, 2, -10, 3, 7, -7, 8, -10, -12, -4, -9, -3, -1, 8, 0, 5, 5, 6, 6, 0, -1, 2, -1, 6, -4, -14, -4, 14, 12, 8, 15, 7, -4, -11, -3, -14, 5, 13, 6, 10, 2, -12, 2, -8, 7, 8, 8, 11, -5, 12, -4, 15, -14, 2, -13, -9, 6, 11, 7, 8, 8, 2, 10, 5, 12, 15, 15, 14, 2, 13, 3, 2, 3, 6, 4, 3, 10, 10, 13, 12, -7, 3, -6, -5, -14, 9, -12, 12, -4, 1, -3, 5, 1, 5, 2, 8, 1, 1, 2, -7, -1, 4, 0, -2, -1, -3, 0, 0, 9, 10, 15, -14, -14, -6, -11, -7, -14, 10, -11, 11, -2, 4, -1, 1, 3, 7, 3, 14, 0, 11, 0, 15, -5, 9, -4, 12, -2, -4, -1, -14, 4, 11, 6, 14, -14, -10, -11, -11, 12, -13, 15, -11, -5, 5, -4, 8, -12, 2, -11, 14, -2, 0, -2, 4, -12, 8, -11, 4, 3, -10, 5, 8, 11, -6, 14, -4, -1, 4, 0, 7, 3, -1, 3, 3, 9, 7, 10, 15, -2, -2, -1, -8, 11, 1, 14, -1, 2, 2, 3, -1, 11, -4, 12, -13, -14, -4, -13, 0, 7, 7, 9, -4, -2, 7, -1, 10, -12, 12, -10, 15, 7, -12, 10, -14, -13, -14, -11, -11, -9, 13, -6, 11, 10, -10, 13, -9, -7, 9, -4, -4, 9, 4, 12, 6, -11, 3, -8, 4, -14, 10, -13, 6, 9, 8, 12, 9, 7, 5, 10, 9, 6, 4, 8, 7, -3, -1, -2, -4, 15, -6, 15, -2, 9, -14, 15, 12, -9, -14, -6, -13, 6, -14, 8, -11, 10, 8, 13, 7, -12, 4, -9, 7, 10, 6, 13, 3, -10, -1, -9, -14, -13, -2, -8, 4, 8, 13, 10, 10, -12, -3, -9, -5, 10, 1, 10, 6, 11, -14, 15, 0, 7, -10, 9, -7, -8, 1, -6, -2, -9, 5, -7, 8, -10, 1, -7, 0, -10, -14, -9, -10, -13, 12, -9, 12, -9, -4, -6, -3, -9, 8, -7, 11, 13, 4, 14, 7, 8, -2, 11, -5, -2, -8, 1, 10, -13, 8, -8, -6, -11, -8, -10, -5, -8, -2, -6, 15, 6, 0, 8, 3, -12, -13, -4, -2, -7, -8, -3, 12, 3, 0, 8, -14, 11, -2, 11, 2, 9, 12, 12, 11, 4, 15, 6, 12, 7, -9, 10, -10, -14, 12, -13, 9, -4, 15, 0, -1, 14, 2, 14, 15, -8, -9, -6, -12, -8, -3, -5, -1, -14, -9, -13, -6, 6, 9, 8, 6, -12, -9, -9, -12, 6, 7, 6, 11, 7, 11, 10, 12, -9, -6, -7, -9, 5, -8, 8, -11, -14, 10, -6, 3, 5, 11, 5, 15, 9, 2, 12, 0, 5, 7, 10, 15, 7, -14, 10, -13, 10, 0, 14, 0, 13, -8, 14, -11, 5, -7, 10, 9, 1, 5, 5, 15, -5, -11, -2, -8, -2, -11, 2, 2, -14, -12, -8, -11, -9, 4, -6, 5, 2, 2, 4, 7, -9, -5, -4, 4, 5, -5, 8, -8, -10, 11, -7, 13, 0, 7, 2, 3, -9, -12, -6, -14, 9, -3, 13, -4, -13, 7, -8, 8, -6, -14, 0, 14, -8, 13, -5, 14, 4, 13, 7, -5, -4, -9, -2, -12, 7, -5, 10, -2, -7, -10, -7, -6, -7, -8, -4, -7, -7, 7, -4, 6, -2, 15, 2, -13, 5, -14, 9, 0, 10, 15, 15, 10, -4, -14, -1, -13, 7, 1, 10, 3, 1, -5, 3, -2, 4, 7, 7, 10, 0, 2, 2, 5, 6, -10, 9, -9, 7, 4, 10, 1, 0, 7, 2, 10, -8, 12, -3, -11, -2, 1, 0, 4, 3, 4, 5, 1, -2, -5, 0, -9, 6, 11, 9, 10, 4, 8, 11, -13, 4, -11, 7, -9, 3, -10, 6, -11, -1, 14, 2, 11, 6, -4, 9, -5, 2, 13, 5, 14, -1, -10, 2, -13, -7, 1, -4, 3, 4, 10, 7, 11, -1, -11, 2, -8, 6, 0, 9, -3, 7, -4, 15, 6, 4, 12, 7, 10, -1, -2, 1, -5, -6, 15, 0, 5, -7, 9, -4, 11, 7, 6, 11, 7, 6, 7, 9, 6, 5, -7, 8, -5, -14, 0, -7, 14, -3, 14, 0, 15, 6, -7, 15, -13, 11, 11, 11, 15
};
//namespace ORB_SLAM
//{
class mdBRIEFextractor
{
public:
enum { HARRIS_SCORE=0, FAST_SCORE=1 };
//mdBRIEFextractor(
// int _nfeatures = 1000,
// double _scaleFactor = 1.2,
// int _nlevels = 8,
// int _scoreType = HARRIS_SCORE,
// int _fastTh = 20,
// bool _learnMask = false,
// bool _useAgast = false,
// int _fastAgastType = 2,
// int _descSize = 32);
mdBRIEFextractor(int _nfeatures = 1000,
float _scaleFactor = 1.2,
int _nlevels = 8,
int _edgeThreshold = 25,
int _firstLevel = 0,
int _scoreType = HARRIS_SCORE,
int _patchSize = 32,
int _fastThreshold = 20,
bool _useAgast = false,
int _fastAgastType = 2,
bool _learnMasks = false,
int _descSize = 32);
~mdBRIEFextractor(){}
// Compute the ORB features and descriptors on an image
void operator()(
cv::InputArray _image,
cv::InputArray _mask,
std::vector<cv::KeyPoint>& _keypoints,
cCamModelGeneral_& camModel,
cv::OutputArray _descriptors,
cv::OutputArray _descriptorMasks);
int inline GetLevels() { return numlevels; }
double inline GetScaleFactor() { return scaleFactor; }
bool GetMasksLearned() { return learnMasks; }
int GetDescriptorSize() { return descSize; }
static int HALF_PATCH_SIZE;
static int EDGE_THRESHOLD;
protected:
// returns the descriptor size in bytes
int descriptorSize() const;
// returns the descriptor type
int descriptorType() const;
// returns the default norm type
int defaultNorm() const;
void ComputePyramid(cv::Mat image, cv::Mat Mask=cv::Mat());
void ComputeKeyPoints(std::vector<std::vector<cv::KeyPoint> >& allKeypoints);
std::vector<cv::Point> pattern;
// int nfeatures;
// double scaleFactor;
// int nlevels;
// int scoreType;
// int fastTh;
//int descSize;
//bool learnMask;
//bool useAgast;
//int fastAgastType;
int nfeatures;
double scaleFactor;
int numlevels;
int edgeThreshold;
int firstLevel;
int descSize;
int scoreType;
int patchSize;
int fastThreshold;
bool useAgast;
int fastAgastType;
bool learnMasks;
int half_patch_size;
std::vector<int> mnFeaturesPerLevel;
std::vector<int> umax;
std::vector<double> mvScaleFactor;
std::vector<double> mvInvScaleFactor;
std::vector<cv::Mat> mvImagePyramid;
std::vector<cv::Mat> mvMaskPyramid;
};
//} //namespace ORB_SLAM
#endif