libfovis
pyramid_level.hpp
1 #ifndef __fovis_pyramid_level_hpp__
2 #define __fovis_pyramid_level_hpp__
3 
4 #include <vector>
5 #include <Eigen/Core>
6 #include <Eigen/Geometry>
7 
8 #include "keypoint.hpp"
9 #include "intensity_descriptor.hpp"
10 #include "grid_filter.hpp"
11 
12 namespace fovis
13 {
14 
22 {
23  public:
24  PyramidLevel(int width, int height, int level_num, int feature_window_size,
25  GridKeyPointFilter& grid_filter);
26  ~PyramidLevel();
27 
28  const uint8_t* getGrayscaleImage() const {
29  return _raw_gray;
30  }
31 
32  int getGrayscaleImageStride() const {
33  return _raw_gray_stride;
34  }
35 
36  const uint8_t* getDescriptor(int i) const {
37  return _descriptors + i * getDescriptorStride();
38  }
39 
40 
41  int getDescriptorStride() const {
42  return _descriptor_extractor->getDescriptorStride();
43  }
44 
45  int getNumKeypoints() const {
46  return _num_keypoints;
47  }
48 
49  int getDescriptorLength() const {
50  return _descriptor_extractor->getDescriptorLength();
51  }
52 
53  const KeyPoint& getKeypoint(int i) const {
54  return _keypoints[i].kp;
55  }
56 
57  const Eigen::Vector4d& getKeypointXYZW(int i) const {
58  return _keypoints[i].xyzw;
59  }
60 
61  float getKeypointRectBaseU(int kp_index) const {
62  return _keypoints[kp_index].rect_base_uv(0);
63  }
64 
65  float getKeypointRectBaseV(int kp_index) const {
66  return _keypoints[kp_index].rect_base_uv(1);
67  }
68 
69  const Eigen::Vector2d& getKeypointRectBaseUV(int kp_index) const {
70  return _keypoints[kp_index].rect_base_uv;
71  }
72 
73  const KeypointData* getKeypointData(int i) const {
74  return &_keypoints[i];
75  }
76 
77  KeypointData* getKeypointData(int i) {
78  return &_keypoints[i];
79  }
80 
81  int getLevelNum() const { return _level_num; }
82 
83  void populateDescriptorInterp(float x, float y, uint8_t* descriptor) const;
84 
85  void populateDescriptorAligned(int x, int y, uint8_t* descriptor) const;
86 
87  void populateDescriptorsInterp(const KeypointData* keypoints,
88  int num_keypoints,
89  uint8_t* descriptors) const;
90 
91  void populateDescriptorsAligned(const KeypointData* keypoints,
92  int num_keypoints,
93  uint8_t* descriptors) const;
94 
95  int getWidth() const { return _width; }
96  int getHeight() const { return _height; }
97 
98  bool isLegalKeypointCoordinate(float x, float y) const {
99  return x >= _keypoint_min_x && x <= _keypoint_max_x &&
100  y >= _keypoint_min_y && y <= _keypoint_max_y;
101  }
102 
103  const int* getDescriptorIndexOffsets() const {
104  return _descriptor_extractor->getDescriptorIndexOffsets();
105  }
106 
107  const std::vector<KeyPoint>& getInitialFeatures() const {
108  return _initial_keypoints;
109  }
110 
111  int getNumDetectedKeypoints() const {
112  return _num_detected_keypoints;
113  }
114 
115  private:
116  friend class OdometryFrame;
117  friend class StereoFrame;
118 
119  void increase_capacity(int new_capacity);
120 
121  uint8_t* _raw_gray;
122  int _raw_gray_stride;
123 
124  std::vector<KeyPoint> _initial_keypoints;
125  int _num_detected_keypoints;
126 
127  GridKeyPointFilter _grid_filter;
128 
129  KeypointData* _keypoints;
130  int _num_keypoints;
131  int _keypoints_capacity;
132 
133  uint8_t* _descriptors;
134 
135  int _keypoint_min_x;
136  int _keypoint_min_y;
137  int _keypoint_max_x;
138  int _keypoint_max_y;
139 
140  int _width;
141  int _height;
142  int _level_num;
143 
144  uint8_t* _pyrbuf;
145 
146  IntensityDescriptorExtractor * _descriptor_extractor;
147 };
148 
149 }
150 #endif
Extracts mean-normalized intensity patch around a pixel.
Definition: intensity_descriptor.hpp:17
Eigen::Vector4d xyzw
Definition: keypoint.hpp:63
const int * getDescriptorIndexOffsets() const
Definition: intensity_descriptor.hpp:95
Eigen::Vector2d rect_base_uv
Definition: keypoint.hpp:85
Places features into grid cells and discards the weakest features in each cell.
Definition: grid_filter.hpp:14
One level of a Gaussian image pyramid.
Definition: pyramid_level.hpp:21
Stores data specific to an input frame.
Definition: frame.hpp:33
int getDescriptorStride() const
Definition: intensity_descriptor.hpp:87
Image feature used for motion estimation.
Definition: keypoint.hpp:49
Stores the right-hand image for a stereo pair.
Definition: stereo_frame.hpp:24
quick and dirty profiling tool.inspired by the matlab tic/toc command
Definition: camera_intrinsics.hpp:6
An interesting point in an image.
Definition: keypoint.hpp:14
KeyPoint kp
Definition: keypoint.hpp:58
int getDescriptorLength() const
Definition: intensity_descriptor.hpp:102