-
Notifications
You must be signed in to change notification settings - Fork 2.5k
/
Tracking.h
375 lines (283 loc) · 9.86 KB
/
Tracking.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
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
/**
* This file is part of ORB-SLAM3
*
* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 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-SLAM3 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-SLAM3.
* If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef TRACKING_H
#define TRACKING_H
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include "Viewer.h"
#include "FrameDrawer.h"
#include "Atlas.h"
#include "LocalMapping.h"
#include "LoopClosing.h"
#include "Frame.h"
#include "ORBVocabulary.h"
#include "KeyFrameDatabase.h"
#include "ORBextractor.h"
#include "MapDrawer.h"
#include "System.h"
#include "ImuTypes.h"
#include "Settings.h"
#include "GeometricCamera.h"
#include <mutex>
#include <unordered_set>
namespace ORB_SLAM3
{
class Viewer;
class FrameDrawer;
class Atlas;
class LocalMapping;
class LoopClosing;
class System;
class Settings;
class Tracking
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Atlas* pAtlas,
KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor, Settings* settings, const string &_nameSeq=std::string());
~Tracking();
// Parse the config file
bool ParseCamParamFile(cv::FileStorage &fSettings);
bool ParseORBParamFile(cv::FileStorage &fSettings);
bool ParseIMUParamFile(cv::FileStorage &fSettings);
// Preprocess the input and call Track(). Extract features and performs stereo matching.
Sophus::SE3f GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double ×tamp, string filename);
Sophus::SE3f GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp, string filename);
Sophus::SE3f GrabImageMonocular(const cv::Mat &im, const double ×tamp, string filename);
void GrabImuData(const IMU::Point &imuMeasurement);
void SetLocalMapper(LocalMapping* pLocalMapper);
void SetLoopClosing(LoopClosing* pLoopClosing);
void SetViewer(Viewer* pViewer);
void SetStepByStep(bool bSet);
bool GetStepByStep();
// Load new settings
// The focal lenght should be similar or scale prediction will fail when projecting points
void ChangeCalibration(const string &strSettingPath);
// Use this function if you have deactivated local mapping and you only want to localize the camera.
void InformOnlyTracking(const bool &flag);
void UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurrentKeyFrame);
KeyFrame* GetLastKeyFrame()
{
return mpLastKeyFrame;
}
void CreateMapInAtlas();
//std::mutex mMutexTracks;
//--
void NewDataset();
int GetNumberDataset();
int GetMatchesInliers();
//DEBUG
void SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf, string strFolder="");
void SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf, Map* pMap);
float GetImageScale();
#ifdef REGISTER_LOOP
void RequestStop();
bool isStopped();
void Release();
bool stopRequested();
#endif
public:
// Tracking states
enum eTrackingState{
SYSTEM_NOT_READY=-1,
NO_IMAGES_YET=0,
NOT_INITIALIZED=1,
OK=2,
RECENTLY_LOST=3,
LOST=4,
OK_KLT=5
};
eTrackingState mState;
eTrackingState mLastProcessedState;
// Input sensor
int mSensor;
// Current Frame
Frame mCurrentFrame;
Frame mLastFrame;
cv::Mat mImGray;
// Initialization Variables (Monocular)
std::vector<int> mvIniLastMatches;
std::vector<int> mvIniMatches;
std::vector<cv::Point2f> mvbPrevMatched;
std::vector<cv::Point3f> mvIniP3D;
Frame mInitialFrame;
// Lists used to recover the full camera trajectory at the end of the execution.
// Basically we store the reference keyframe for each frame and its relative transformation
list<Sophus::SE3f> mlRelativeFramePoses;
list<KeyFrame*> mlpReferences;
list<double> mlFrameTimes;
list<bool> mlbLost;
// frames with estimated pose
int mTrackedFr;
bool mbStep;
// True if local mapping is deactivated and we are performing only localization
bool mbOnlyTracking;
void Reset(bool bLocMap = false);
void ResetActiveMap(bool bLocMap = false);
float mMeanTrack;
bool mbInitWith3KFs;
double t0; // time-stamp of first read frame
double t0vis; // time-stamp of first inserted keyframe
double t0IMU; // time-stamp of IMU initialization
bool mFastInit = false;
vector<MapPoint*> GetLocalMapMPS();
bool mbWriteStats;
#ifdef REGISTER_TIMES
void LocalMapStats2File();
void TrackStats2File();
void PrintTimeStats();
vector<double> vdRectStereo_ms;
vector<double> vdResizeImage_ms;
vector<double> vdORBExtract_ms;
vector<double> vdStereoMatch_ms;
vector<double> vdIMUInteg_ms;
vector<double> vdPosePred_ms;
vector<double> vdLMTrack_ms;
vector<double> vdNewKF_ms;
vector<double> vdTrackTotal_ms;
#endif
protected:
// Main tracking function. It is independent of the input sensor.
void Track();
// Map initialization for stereo and RGB-D
void StereoInitialization();
// Map initialization for monocular
void MonocularInitialization();
//void CreateNewMapPoints();
void CreateInitialMapMonocular();
void CheckReplacedInLastFrame();
bool TrackReferenceKeyFrame();
void UpdateLastFrame();
bool TrackWithMotionModel();
bool PredictStateIMU();
bool Relocalization();
void UpdateLocalMap();
void UpdateLocalPoints();
void UpdateLocalKeyFrames();
bool TrackLocalMap();
void SearchLocalPoints();
bool NeedNewKeyFrame();
void CreateNewKeyFrame();
// Perform preintegration from last frame
void PreintegrateIMU();
// Reset IMU biases and compute frame velocity
void ResetFrameIMU();
bool mbMapUpdated;
// Imu preintegration from last frame
IMU::Preintegrated *mpImuPreintegratedFromLastKF;
// Queue of IMU measurements between frames
std::list<IMU::Point> mlQueueImuData;
// Vector of IMU measurements from previous to current frame (to be filled by PreintegrateIMU)
std::vector<IMU::Point> mvImuFromLastFrame;
std::mutex mMutexImuQueue;
// Imu calibration parameters
IMU::Calib *mpImuCalib;
// Last Bias Estimation (at keyframe creation)
IMU::Bias mLastBias;
// In case of performing only localization, this flag is true when there are no matches to
// points in the map. Still tracking will continue if there are enough matches with temporal points.
// In that case we are doing visual odometry. The system will try to do relocalization to recover
// "zero-drift" localization to the map.
bool mbVO;
//Other Thread Pointers
LocalMapping* mpLocalMapper;
LoopClosing* mpLoopClosing;
//ORB
ORBextractor* mpORBextractorLeft, *mpORBextractorRight;
ORBextractor* mpIniORBextractor;
//BoW
ORBVocabulary* mpORBVocabulary;
KeyFrameDatabase* mpKeyFrameDB;
// Initalization (only for monocular)
bool mbReadyToInitializate;
bool mbSetInit;
//Local Map
KeyFrame* mpReferenceKF;
std::vector<KeyFrame*> mvpLocalKeyFrames;
std::vector<MapPoint*> mvpLocalMapPoints;
// System
System* mpSystem;
//Drawers
Viewer* mpViewer;
FrameDrawer* mpFrameDrawer;
MapDrawer* mpMapDrawer;
bool bStepByStep;
//Atlas
Atlas* mpAtlas;
//Calibration matrix
cv::Mat mK;
Eigen::Matrix3f mK_;
cv::Mat mDistCoef;
float mbf;
float mImageScale;
float mImuFreq;
double mImuPer;
bool mInsertKFsLost;
//New KeyFrame rules (according to fps)
int mMinFrames;
int mMaxFrames;
int mnFirstImuFrameId;
int mnFramesToResetIMU;
// Threshold close/far points
// Points seen as close by the stereo/RGBD sensor are considered reliable
// and inserted from just one frame. Far points requiere a match in two keyframes.
float mThDepth;
// For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are scaled.
float mDepthMapFactor;
//Current matches in frame
int mnMatchesInliers;
//Last Frame, KeyFrame and Relocalisation Info
KeyFrame* mpLastKeyFrame;
unsigned int mnLastKeyFrameId;
unsigned int mnLastRelocFrameId;
double mTimeStampLost;
double time_recently_lost;
unsigned int mnFirstFrameId;
unsigned int mnInitialFrameId;
unsigned int mnLastInitFrameId;
bool mbCreatedMap;
//Motion Model
bool mbVelocity{false};
Sophus::SE3f mVelocity;
//Color order (true RGB, false BGR, ignored if grayscale)
bool mbRGB;
list<MapPoint*> mlpTemporalPoints;
//int nMapChangeIndex;
int mnNumDataset;
ofstream f_track_stats;
ofstream f_track_times;
double mTime_PreIntIMU;
double mTime_PosePred;
double mTime_LocalMapTrack;
double mTime_NewKF_Dec;
GeometricCamera* mpCamera, *mpCamera2;
int initID, lastID;
Sophus::SE3f mTlr;
void newParameterLoader(Settings* settings);
#ifdef REGISTER_LOOP
bool Stop();
bool mbStopped;
bool mbStopRequested;
bool mbNotStop;
std::mutex mMutexStop;
#endif
public:
cv::Mat mImRight;
};
} //namespace ORB_SLAM
#endif // TRACKING_H