203 lines
4.8 KiB
C++
203 lines
4.8 KiB
C++
/**
|
||
* This file is part of ORB-SLAM3
|
||
*
|
||
* Copyright (C) 2017-2020 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 LOCALMAPPING_H
|
||
#define LOCALMAPPING_H
|
||
|
||
#include "KeyFrame.h"
|
||
#include "Atlas.h"
|
||
#include "LoopClosing.h"
|
||
#include "Tracking.h"
|
||
#include "KeyFrameDatabase.h"
|
||
#include "Initializer.h"
|
||
|
||
#include <mutex>
|
||
|
||
|
||
namespace ORB_SLAM3
|
||
{
|
||
|
||
class System;
|
||
class Tracking;
|
||
class LoopClosing;
|
||
class Atlas;
|
||
|
||
class LocalMapping
|
||
{
|
||
public:
|
||
LocalMapping(System* pSys, Atlas* pAtlas, const float bMonocular, bool bInertial, const string &_strSeqName=std::string());
|
||
|
||
void SetLoopCloser(LoopClosing* pLoopCloser);
|
||
|
||
void SetTracker(Tracking* pTracker);
|
||
|
||
// Main function
|
||
void Run();
|
||
|
||
void InsertKeyFrame(KeyFrame* pKF);
|
||
void EmptyQueue();
|
||
|
||
// Thread Synch
|
||
void RequestStop();
|
||
void RequestReset();
|
||
void RequestResetActiveMap(Map* pMap);
|
||
bool Stop();
|
||
void Release();
|
||
bool isStopped();
|
||
bool stopRequested();
|
||
bool AcceptKeyFrames();
|
||
void SetAcceptKeyFrames(bool flag);
|
||
bool SetNotStop(bool flag);
|
||
|
||
void InterruptBA();
|
||
|
||
void RequestFinish();
|
||
bool isFinished();
|
||
|
||
int KeyframesInQueue(){
|
||
unique_lock<std::mutex> lock(mMutexNewKFs);
|
||
return mlNewKeyFrames.size();
|
||
}
|
||
|
||
bool IsInitializing();
|
||
double GetCurrKFTime();
|
||
KeyFrame* GetCurrKF();
|
||
|
||
std::mutex mMutexImuInit;
|
||
|
||
Eigen::MatrixXd mcovInertial;
|
||
Eigen::Matrix3d mRwg;
|
||
Eigen::Vector3d mbg;
|
||
Eigen::Vector3d mba;
|
||
double mScale;
|
||
double mInitTime;
|
||
double mCostTime;
|
||
bool mbNewInit;
|
||
unsigned int mInitSect;
|
||
unsigned int mIdxInit;
|
||
unsigned int mnKFs;
|
||
double mFirstTs;
|
||
int mnMatchesInliers;
|
||
|
||
bool mbNotBA1;
|
||
bool mbNotBA2;
|
||
bool mbBadImu;
|
||
|
||
bool mbWriteStats;
|
||
|
||
// not consider far points (clouds)
|
||
bool mbFarPoints;
|
||
float mThFarPoints;
|
||
|
||
#ifdef REGISTER_TIMES
|
||
vector<double> vdKFInsert_ms;
|
||
vector<double> vdMPCulling_ms;
|
||
vector<double> vdMPCreation_ms;
|
||
vector<double> vdLBA_ms;
|
||
vector<double> vdKFCulling_ms;
|
||
vector<double> vdLMTotal_ms;
|
||
|
||
|
||
vector<double> vdLBASync_ms;
|
||
vector<double> vdKFCullingSync_ms;
|
||
vector<int> vnLBA_edges;
|
||
vector<int> vnLBA_KFopt;
|
||
vector<int> vnLBA_KFfixed;
|
||
vector<int> vnLBA_MPs;
|
||
int nLBA_exec;
|
||
int nLBA_abort;
|
||
#endif
|
||
protected:
|
||
|
||
bool CheckNewKeyFrames();
|
||
void ProcessNewKeyFrame();
|
||
void CreateNewMapPoints();
|
||
|
||
void MapPointCulling();
|
||
void SearchInNeighbors();
|
||
void KeyFrameCulling();
|
||
|
||
cv::Mat ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2);
|
||
cv::Matx33f ComputeF12_(KeyFrame* &pKF1, KeyFrame* &pKF2);
|
||
|
||
cv::Mat SkewSymmetricMatrix(const cv::Mat &v);
|
||
cv::Matx33f SkewSymmetricMatrix_(const cv::Matx31f &v);
|
||
|
||
System *mpSystem;
|
||
|
||
bool mbMonocular; //mSensor==MONOCULAR || mSensor==IMU_MONOCULAR
|
||
bool mbInertial; //mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO
|
||
|
||
void ResetIfRequested();
|
||
bool mbResetRequested;
|
||
bool mbResetRequestedActiveMap;
|
||
Map* mpMapToReset;
|
||
std::mutex mMutexReset;
|
||
|
||
bool CheckFinish();
|
||
void SetFinish();
|
||
bool mbFinishRequested;
|
||
bool mbFinished;
|
||
std::mutex mMutexFinish;
|
||
|
||
Atlas* mpAtlas;
|
||
|
||
LoopClosing* mpLoopCloser;
|
||
Tracking* mpTracker;
|
||
|
||
std::list<KeyFrame*> mlNewKeyFrames;
|
||
|
||
KeyFrame* mpCurrentKeyFrame;
|
||
|
||
std::list<MapPoint*> mlpRecentAddedMapPoints;
|
||
|
||
std::mutex mMutexNewKFs;
|
||
|
||
bool mbAbortBA;
|
||
|
||
bool mbStopped;
|
||
bool mbStopRequested;
|
||
bool mbNotStop;
|
||
std::mutex mMutexStop;
|
||
|
||
bool mbAcceptKeyFrames;
|
||
std::mutex mMutexAccept;
|
||
// IMU初始化函数,通过控制不同的参数来表示不同阶段
|
||
void InitializeIMU(float priorG = 1e2, float priorA = 1e6, bool bFirst = false);
|
||
// 单目惯性模式下优化尺度和重力方向
|
||
void ScaleRefinement();
|
||
//跟踪线程使用,如果为true,暂不添加关键帧
|
||
bool bInitializing;
|
||
|
||
Eigen::MatrixXd infoInertial;
|
||
int mNumLM;
|
||
int mNumKFCulling;
|
||
|
||
float mTinit;
|
||
|
||
int countRefinement;
|
||
|
||
//DEBUG
|
||
ofstream f_lm;
|
||
};
|
||
|
||
} //namespace ORB_SLAM
|
||
|
||
#endif // LOCALMAPPING_H
|