24 #include<opencv2/core/core.hpp> 25 #include<opencv2/features2d/features2d.hpp> 48 mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc),
49 mpKeyFrameDB(pKFDB), mpInitializer(static_cast<
Initializer*>(NULL)), mpSystem(pSys),
50 mpFrameDrawer(pFrameDrawer), mpMap(pMap), mnLastRelocFrameId(0), mnMinimumKeyFrames(5)
63 cv::Mat K = cv::Mat::eye(3,3,CV_32F);
64 K.at<
float>(0,0) = parameters.
fx;
65 K.at<
float>(1,1) = parameters.
fy;
66 K.at<
float>(0,2) = parameters.
cx;
67 K.at<
float>(1,2) = parameters.
cy;
70 cv::Mat DistCoef(4,1,CV_32F);
71 DistCoef.at<
float>(0) = parameters.
k1;
72 DistCoef.at<
float>(1) = parameters.
k2;
73 DistCoef.at<
float>(2) = parameters.
p1;
74 DistCoef.at<
float>(3) = parameters.
p2;
78 DistCoef.at<
float>(4) = parameters.
k3;
87 cout << endl <<
"Camera Parameters: " << endl;
88 cout <<
"- fx: " << parameters.
fx << endl;
89 cout <<
"- fy: " << parameters.
fy << endl;
90 cout <<
"- cx: " << parameters.
cx << endl;
91 cout <<
"- cy: " << parameters.
cy << endl;
92 cout <<
"- k1: " << DistCoef.at<
float>(0) << endl;
93 cout <<
"- k2: " << DistCoef.at<
float>(1) << endl;
95 cout <<
"- k3: " << DistCoef.at<
float>(4) << endl;
96 cout <<
"- p1: " << DistCoef.at<
float>(2) << endl;
97 cout <<
"- p2: " << DistCoef.at<
float>(3) << endl;
99 cout <<
"- bf: " <<
mbf << endl;
102 cout <<
"- color order: RGB (ignored if grayscale)" << endl;
104 cout <<
"- color order: BGR (ignored if grayscale)" << endl;
114 cout << endl <<
"ORB Extractor Parameters: " << endl;
115 cout <<
"- Number of Features: " <<
nFeatures << endl;
116 cout <<
"- Scale Levels: " <<
nLevels << endl;
118 cout <<
"- Initial Fast Threshold: " <<
fIniThFAST << endl;
119 cout <<
"- Minimum Fast Threshold: " <<
fMinThFAST << endl;
124 cout << endl <<
"Depth Threshold (Close/Far Points): " <<
mThDepth << endl;
151 cv::Mat imGrayRight = imRectRight;
158 cvtColor(imGrayRight,imGrayRight,CV_RGB2GRAY);
163 cvtColor(imGrayRight,imGrayRight,CV_BGR2GRAY);
171 cvtColor(imGrayRight,imGrayRight,CV_RGBA2GRAY);
176 cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY);
191 cv::Mat imDepth = imD;
336 bool bOKReloc =
false;
337 vector<MapPoint*> vpMPsMM;
338 vector<bool> vbOutMM;
349 if(bOKMM && !bOKReloc)
371 bOK = bOKReloc || bOKMM;
407 cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
457 cout <<
"Track lost soon after initialisation, reseting..." << endl;
593 vector<bool> vbTriangulated;
608 cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
609 Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
610 tcw.copyTo(Tcw.rowRange(0,3).col(3));
671 float invMedianDepth = 1.0f/medianDepth;
673 if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
675 cout <<
"Wrong initialization, reseting..." << endl;
681 cv::Mat Tc2w = pKFcur->
GetPose();
682 Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
687 for(
size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++)
689 if(vpAllMapPoints[iMP])
691 MapPoint* pMP = vpAllMapPoints[iMP];
746 vector<MapPoint*> vpMapPointMatches;
779 return nmatchesMap>=10;
795 vector<pair<float,int> > vDepthIdx;
802 vDepthIdx.push_back(make_pair(z,i));
806 if(vDepthIdx.empty())
809 sort(vDepthIdx.begin(),vDepthIdx.end());
814 for(
size_t j=0; j<vDepthIdx.size();j++)
816 int i = vDepthIdx[j].second;
818 bool bCreateNew =
false;
843 if(vDepthIdx[j].first>
mThDepth && nPoints>100)
904 mbVO = nmatchesMap<10;
908 return nmatchesMap>=10;
983 int nNonTrackedClose = 0;
984 int nTrackedClose= 0;
999 bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);
1002 float thRefRatio = 0.75f;
1016 const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);
1018 if((c1a||c1b||c1c)&&c2)
1022 if(bLocalMappingIdle)
1061 vector<pair<float,int> > vDepthIdx;
1068 vDepthIdx.push_back(make_pair(z,i));
1072 if(!vDepthIdx.empty())
1074 sort(vDepthIdx.begin(),vDepthIdx.end());
1077 for(
size_t j=0; j<vDepthIdx.size();j++)
1079 int i = vDepthIdx[j].second;
1081 bool bCreateNew =
false;
1110 if(vDepthIdx[j].first>
mThDepth && nPoints>100)
1134 *vit =
static_cast<MapPoint*
>(NULL);
1195 for(vector<MapPoint*>::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++)
1215 map<KeyFrame*,int> keyframeCounter;
1224 for(map<KeyFrame*,size_t>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++)
1225 keyframeCounter[it->first]++;
1234 if(keyframeCounter.empty())
1244 for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++)
1273 for(vector<KeyFrame*>::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++)
1276 if(!pNeighKF->
isBad())
1287 const set<KeyFrame*> spChilds = pKF->
GetChilds();
1288 for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++)
1291 if(!pChildKF->
isBad())
1331 if(vpCandidateKFs.empty())
1334 const int nKFs = vpCandidateKFs.size();
1340 vector<PnPsolver*> vpPnPsolvers;
1341 vpPnPsolvers.resize(nKFs);
1343 vector<vector<MapPoint*> > vvpMapPointMatches;
1344 vvpMapPointMatches.resize(nKFs);
1346 vector<bool> vbDiscarded;
1347 vbDiscarded.resize(nKFs);
1351 for(
int i=0; i<nKFs; i++)
1355 vbDiscarded[i] =
true;
1361 vbDiscarded[i] =
true;
1368 vpPnPsolvers[i] = pSolver;
1376 bool bMatch =
false;
1379 while(nCandidates>0 && !bMatch)
1381 for(
int i=0; i<nKFs; i++)
1387 vector<bool> vbInliers;
1392 cv::Mat Tcw = pSolver->
iterate(5,bNoMore,vbInliers,nInliers);
1397 vbDiscarded[i]=
true;
1406 set<MapPoint*> sFound;
1408 const int np = vbInliers.size();
1410 for(
int j=0; j<np; j++)
1415 sFound.insert(vvpMapPointMatches[i][j]);
1435 if(nadditional+nGood>=50)
1441 if(nGood>30 && nGood<50)
1450 if(nGood+nadditional>=50)
1489 cout <<
"System Reseting" << endl;
1492 cout <<
"Reseting Local Mapper...";
1494 cout <<
" done" << endl;
1497 cout <<
"Reseting Loop Closing...";
1499 cout <<
" done" << endl;
1502 cout <<
"Reseting Database...";
1504 cout <<
" done" << endl;
1528 cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
1529 float fx = fSettings[
"Camera.fx"];
1530 float fy = fSettings[
"Camera.fy"];
1531 float cx = fSettings[
"Camera.cx"];
1532 float cy = fSettings[
"Camera.cy"];
1534 cv::Mat K = cv::Mat::eye(3,3,CV_32F);
1535 K.at<
float>(0,0) = fx;
1536 K.at<
float>(1,1) = fy;
1537 K.at<
float>(0,2) = cx;
1538 K.at<
float>(1,2) = cy;
1541 cv::Mat DistCoef(4,1,CV_32F);
1542 DistCoef.at<
float>(0) = fSettings[
"Camera.k1"];
1543 DistCoef.at<
float>(1) = fSettings[
"Camera.k2"];
1544 DistCoef.at<
float>(2) = fSettings[
"Camera.p1"];
1545 DistCoef.at<
float>(3) = fSettings[
"Camera.p2"];
1546 const float k3 = fSettings[
"Camera.k3"];
1550 DistCoef.at<
float>(4) = k3;
1554 mbf = fSettings[
"Camera.bf"];
std::vector< KeyFrame * > GetBestCovisibilityKeyFrames(const int &N)
std::vector< MapPoint * > mvpLocalMapPoints
std::vector< KeyFrame * > mvpLocalKeyFrames
unsigned int mnLastKeyFrameId
std::vector< MapPoint * > mvpMapPoints
unsigned int mnLastRelocFrameId
std::vector< KeyFrame * > DetectRelocalizationCandidates(Frame *F)
long unsigned int mnTrackReferenceForFrame
void SetLoopClosing(LoopClosing *pLoopClosing)
int SearchByBoW(KeyFrame *pKF, Frame &F, std::vector< MapPoint * > &vpMapPointMatches)
KeyFrame * mpLastKeyFrame
bool TrackWithMotionModel()
std::vector< int > mvIniMatches
void AddMapPoint(MapPoint *pMP, const size_t &idx)
void SetLocalMapper(LocalMapping *pLocalMapper)
cv::Mat GrabImageRGBD(const cv::Mat &imRGB, const cv::Mat &imD, const double ×tamp)
std::mutex mMutexMapUpdate
cv::Mat UnprojectStereo(const int &i)
Initializer * mpInitializer
std::vector< MapPoint * > GetMapPointMatches()
std::vector< bool > mvbOutlier
void IncreaseVisible(int n=1)
bool SetNotStop(bool flag)
cv::Mat GetRotationInverse()
void UpdatePoseMatrices()
FrameDrawer * mpFrameDrawer
cv::Mat iterate(int nIterations, bool &bNoMore, vector< bool > &vbInliers, int &nInliers)
void AddObservation(KeyFrame *pKF, size_t idx)
ORBVocabulary * mpORBVocabulary
int TrackedMapPoints(const int &minObs)
KeyFrameDatabase * mpKeyFrameDB
cv::Mat GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double ×tamp)
void UpdateNormalAndDepth()
void AddMapPoint(MapPoint *pMP)
std::vector< cv::KeyPoint > mvKeysUn
std::vector< MapPoint * > GetAllMapPoints()
cv::Mat GetCameraCenter()
vector< KeyFrame * > mvpKeyFrameOrigins
void MonocularInitialization()
static void GlobalBundleAdjustemnt(Map *pMap, int nIterations=5, bool *pbStopFlag=NULL, const unsigned long nLoopKF=0, const bool bRobust=true)
std::map< KeyFrame *, size_t > GetObservations()
void InsertKeyFrame(KeyFrame *pKF)
bool isInFrustum(MapPoint *pMP, float viewingCosLimit)
void CreateInitialMapMonocular()
std::vector< float > mvDepth
long unsigned KeyFramesInMap()
static bool mbInitialComputations
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)
void ChangeCalibration(const string &strSettingPath)
LoopClosing * mpLoopClosing
list< double > mlFrameTimes
static long unsigned int nNextId
void SetWorldPos(const cv::Mat &Pos)
long unsigned int MapPointsInMap()
ORBextractor * mpORBextractorLeft
int SearchByProjection(Frame &F, const std::vector< MapPoint * > &vpMapPoints, const float th=3)
std::vector< cv::KeyPoint > mvKeys
static int PoseOptimization(Frame *pFrame)
void InformOnlyTracking(const bool &flag)
int SearchForInitialization(Frame &F1, Frame &F2, std::vector< cv::Point2f > &vbPrevMatched, std::vector< int > &vnMatches12, int windowSize=10)
float ComputeSceneMedianDepth(const int q)
long unsigned int mnLastFrameSeen
void SetPose(const cv::Mat &Tcw)
TFSIMD_FORCE_INLINE const tfScalar & z() const
ORBextractor * mpIniORBextractor
void SetRansacParameters(double probability=0.99, int minInliers=8, int maxIterations=300, int minSet=4, float epsilon=0.4, float th2=5.991)
list< KeyFrame * > mlpReferences
void CheckReplacedInLastFrame()
void StereoInitialization()
eTrackingState mLastProcessedState
void Update(Tracking *pTracker)
std::set< KeyFrame * > GetChilds()
bool TrackReferenceKeyFrame()
static long unsigned int nNextId
void SetPose(cv::Mat Tcw)
void SetReferenceMapPoints(const std::vector< MapPoint * > &vpMPs)
void AddKeyFrame(KeyFrame *pKF)
list< MapPoint * > mlpTemporalPoints
bool Initialize(const Frame &CurrentFrame, const vector< int > &vMatches12, cv::Mat &R21, cv::Mat &t21, vector< cv::Point3f > &vP3D, vector< bool > &vbTriangulated)
long unsigned int mnTrackReferenceForFrame
list< cv::Mat > mlRelativeFramePoses
cv::Mat GrabImageMonocular(const cv::Mat &im, const double ×tamp)
void UpdateLocalKeyFrames()
LocalMapping * mpLocalMapper
std::vector< cv::Point3f > mvIniP3D
void ComputeDistinctiveDescriptors()
std::vector< cv::Point2f > mvbPrevMatched
ORBextractor * mpORBextractorRight