forked from raulmur/ORB_SLAM2
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Frame.h
213 lines (161 loc) · 6.72 KB
/
Frame.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
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 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-SLAM2 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-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef FRAME_H
#define FRAME_H
#include<vector>
#include "MapPoint.h"
#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
#include "ORBVocabulary.h"
#include "KeyFrame.h"
#include "ORBextractor.h"
#include <opencv2/opencv.hpp>
namespace ORB_SLAM2
{
#define FRAME_GRID_ROWS 48
#define FRAME_GRID_COLS 64
class MapPoint;
class KeyFrame;
class Frame
{
public:
Frame();
// Copy constructor.
Frame(const Frame &frame);
// Constructor for stereo cameras.
Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
// Constructor for RGB-D cameras.
Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
// Constructor for Monocular cameras.
Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
// Extract ORB on the image. 0 for left image and 1 for right image.
void ExtractORB(int flag, const cv::Mat &im);
// Compute Bag of Words representation.
void ComputeBoW();
// Set the camera pose.
void SetPose(cv::Mat Tcw);
// Computes rotation, translation and camera center matrices from the camera pose.
void UpdatePoseMatrices();
// Returns the camera center.
inline cv::Mat GetCameraCenter(){
return mOw.clone();
}
// Returns inverse of rotation
inline cv::Mat GetRotationInverse(){
return mRwc.clone();
}
// Check if a MapPoint is in the frustum of the camera
// and fill variables of the MapPoint to be used by the tracking
bool isInFrustum(MapPoint* pMP, float viewingCosLimit);
// Compute the cell of a keypoint (return false if outside the grid)
bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY);
vector<size_t> GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel=-1, const int maxLevel=-1) const;
// Search a match for each keypoint in the left image to a keypoint in the right image.
// If there is a match, depth is computed and the right coordinate associated to the left keypoint is stored.
void ComputeStereoMatches();
// Associate a "right" coordinate to a keypoint if there is valid depth in the depthmap.
void ComputeStereoFromRGBD(const cv::Mat &imDepth);
// Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates.
cv::Mat UnprojectStereo(const int &i);
public:
// Vocabulary used for relocalization.
ORBVocabulary* mpORBvocabulary;
// Feature extractor. The right is used only in the stereo case.
ORBextractor* mpORBextractorLeft, *mpORBextractorRight;
// Frame timestamp.
double mTimeStamp;
// Calibration matrix and OpenCV distortion parameters.
cv::Mat mK;
static float fx;
static float fy;
static float cx;
static float cy;
static float invfx;
static float invfy;
cv::Mat mDistCoef;
// Stereo baseline multiplied by fx.
float mbf;
// Stereo baseline in meters.
float mb;
// Threshold close/far points. Close points are inserted from 1 view.
// Far points are inserted as in the monocular case from 2 views.
float mThDepth;
// Number of KeyPoints.
int N;
// Vector of keypoints (original for visualization) and undistorted (actually used by the system).
// In the stereo case, mvKeysUn is redundant as images must be rectified.
// In the RGB-D case, RGB images can be distorted.
std::vector<cv::KeyPoint> mvKeys, mvKeysRight;
std::vector<cv::KeyPoint> mvKeysUn;
// Corresponding stereo coordinate and depth for each keypoint.
// "Monocular" keypoints have a negative value.
std::vector<float> mvuRight;
std::vector<float> mvDepth;
// Bag of Words Vector structures.
DBoW2::BowVector mBowVec;
DBoW2::FeatureVector mFeatVec;
// ORB descriptor, each row associated to a keypoint.
cv::Mat mDescriptors, mDescriptorsRight;
// MapPoints associated to keypoints, NULL pointer if no association.
std::vector<MapPoint*> mvpMapPoints;
// Flag to identify outlier associations.
std::vector<bool> mvbOutlier;
// Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints.
static float mfGridElementWidthInv;
static float mfGridElementHeightInv;
std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];
// Camera pose.
cv::Mat mTcw;
// Current and Next Frame id.
static long unsigned int nNextId;
long unsigned int mnId;
// Reference Keyframe.
KeyFrame* mpReferenceKF;
// Scale pyramid info.
int mnScaleLevels;
float mfScaleFactor;
float mfLogScaleFactor;
vector<float> mvScaleFactors;
vector<float> mvInvScaleFactors;
vector<float> mvLevelSigma2;
vector<float> mvInvLevelSigma2;
// Undistorted Image Bounds (computed once).
static float mnMinX;
static float mnMaxX;
static float mnMinY;
static float mnMaxY;
static bool mbInitialComputations;
private:
// Undistort keypoints given OpenCV distortion parameters.
// Only for the RGB-D case. Stereo must be already rectified!
// (called in the constructor).
void UndistortKeyPoints();
// Computes image bounds for the undistorted image (called in the constructor).
void ComputeImageBounds(const cv::Mat &imLeft);
// Assign keypoints to the grid for speed up feature matching (called in the constructor).
void AssignFeaturesToGrid();
// Rotation, translation and camera center
cv::Mat mRcw;
cv::Mat mtcw;
cv::Mat mRwc;
cv::Mat mOw; //==mtwc
};
}// namespace ORB_SLAM
#endif // FRAME_H