Tracking.cc
Go to the documentation of this file.
1 
22 #include "Tracking.h"
23 
24 #include<opencv2/core/core.hpp>
25 #include<opencv2/features2d/features2d.hpp>
26 
27 #include"ORBmatcher.h"
28 #include"FrameDrawer.h"
29 #include"Converter.h"
30 #include"Map.h"
31 #include"Initializer.h"
32 
33 #include"Optimizer.h"
34 #include"PnPsolver.h"
35 
36 #include<iostream>
37 
38 #include<mutex>
39 
40 
41 using namespace std;
42 
43 namespace ORB_SLAM2
44 {
45 
46 Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer,
47  Map *pMap, KeyFrameDatabase* pKFDB, const int sensor, ORBParameters& parameters):
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)
51 {
52  //Unpack all the parameters from the parameters struct (this replaces loading in a second configuration file)
53  mMaxFrames = parameters.maxFrames;
54  mbRGB = parameters.RGB;
55  mThDepth = parameters.thDepth;
56  nFeatures = parameters.nFeatures;
57  fScaleFactor = parameters.scaleFactor;
58  nLevels = parameters.nLevels;
59  fIniThFAST = parameters.iniThFAST;
60  fMinThFAST = parameters.minThFAST;
61  mDepthMapFactor = parameters.depthMapFactor;
62 
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;
68  K.copyTo(mK);
69 
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;
75  if(parameters.k3!=0)
76  {
77  DistCoef.resize(5);
78  DistCoef.at<float>(4) = parameters.k3;
79  }
80  DistCoef.copyTo(mDistCoef);
81 
82  mbf = parameters.baseline;
83 
84  // Max/Min Frames to insert keyframes and to check relocalization
85  mMinFrames = 0;
86 
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;
94  if(DistCoef.rows==5)
95  cout << "- k3: " << DistCoef.at<float>(4) << endl;
96  cout << "- p1: " << DistCoef.at<float>(2) << endl;
97  cout << "- p2: " << DistCoef.at<float>(3) << endl;
98  cout << "- fps: " << mMaxFrames << endl;
99  cout << "- bf: " << mbf << endl;
100 
101  if(mbRGB)
102  cout << "- color order: RGB (ignored if grayscale)" << endl;
103  else
104  cout << "- color order: BGR (ignored if grayscale)" << endl;
105 
107 
108  if(sensor==System::STEREO)
110 
111  if(sensor==System::MONOCULAR)
113 
114  cout << endl << "ORB Extractor Parameters: " << endl;
115  cout << "- Number of Features: " << nFeatures << endl;
116  cout << "- Scale Levels: " << nLevels << endl;
117  cout << "- Scale Factor: " << fScaleFactor << endl;
118  cout << "- Initial Fast Threshold: " << fIniThFAST << endl;
119  cout << "- Minimum Fast Threshold: " << fMinThFAST << endl;
120 
121  if(sensor==System::STEREO || sensor==System::RGBD)
122  {
123  mThDepth = mbf*(float)mThDepth/parameters.fx;
124  cout << endl << "Depth Threshold (Close/Far Points): " << mThDepth << endl;
125  }
126 
127  if(sensor==System::RGBD)
128  {
129  if(fabs(mDepthMapFactor)<1e-5)
130  mDepthMapFactor=1;
131  else
133  }
134 
135 }
136 
138 {
139  mpLocalMapper=pLocalMapper;
140 }
141 
143 {
144  mpLoopClosing=pLoopClosing;
145 }
146 
147 
148 cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double &timestamp)
149 {
150  mImGray = imRectLeft;
151  cv::Mat imGrayRight = imRectRight;
152 
153  if(mImGray.channels()==3)
154  {
155  if(mbRGB)
156  {
157  cvtColor(mImGray,mImGray,CV_RGB2GRAY);
158  cvtColor(imGrayRight,imGrayRight,CV_RGB2GRAY);
159  }
160  else
161  {
162  cvtColor(mImGray,mImGray,CV_BGR2GRAY);
163  cvtColor(imGrayRight,imGrayRight,CV_BGR2GRAY);
164  }
165  }
166  else if(mImGray.channels()==4)
167  {
168  if(mbRGB)
169  {
170  cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
171  cvtColor(imGrayRight,imGrayRight,CV_RGBA2GRAY);
172  }
173  else
174  {
175  cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
176  cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY);
177  }
178  }
179 
181 
182  Track();
183 
184  return mCurrentFrame.mTcw.clone();
185 }
186 
187 
188 cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp)
189 {
190  mImGray = imRGB;
191  cv::Mat imDepth = imD;
192 
193  if(mImGray.channels()==3)
194  {
195  if(mbRGB)
196  cvtColor(mImGray,mImGray,CV_RGB2GRAY);
197  else
198  cvtColor(mImGray,mImGray,CV_BGR2GRAY);
199  }
200  else if(mImGray.channels()==4)
201  {
202  if(mbRGB)
203  cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
204  else
205  cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
206  }
207 
208  if((fabs(mDepthMapFactor-1.0f)>1e-5) || imDepth.type()!=CV_32F)
209  imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor);
210 
212 
213  Track();
214 
215  return mCurrentFrame.mTcw.clone();
216 }
217 
218 
219 cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp)
220 {
221  mImGray = im;
222 
223  if(mImGray.channels()==3)
224  {
225  if(mbRGB)
226  cvtColor(mImGray,mImGray,CV_RGB2GRAY);
227  else
228  cvtColor(mImGray,mImGray,CV_BGR2GRAY);
229  }
230  else if(mImGray.channels()==4)
231  {
232  if(mbRGB)
233  cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
234  else
235  cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
236  }
237 
240  else
242 
243  Track();
244 
245  return mCurrentFrame.mTcw.clone();
246 }
247 
249 {
250  if(mState==NO_IMAGES_YET)
251  {
253  }
254 
256 
257  // Get Map Mutex -> Map cannot be changed
258  unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
259 
261  {
264  else
266 
267  mpFrameDrawer->Update(this);
268 
269  if(mState!=OK)
270  return;
271  }
272  else
273  {
274  // System is initialized. Track Frame.
275  bool bOK;
276 
277  // Initial camera pose estimation using motion model or relocalization (if tracking is lost)
278  if(!mbOnlyTracking)
279  {
280  // Local Mapping is activated. This is the normal behaviour, unless
281  // you explicitly activate the "only tracking" mode.
282 
283  if(mState==OK)
284  {
285  // Local Mapping might have changed some MapPoints tracked in last frame
287 
289  {
290  bOK = TrackReferenceKeyFrame();
291  }
292  else
293  {
294  bOK = TrackWithMotionModel();
295  if(!bOK)
296  bOK = TrackReferenceKeyFrame();
297  }
298  }
299  else
300  {
301  bOK = Relocalization();
302  }
303  }
304  else
305  {
306  // Localization Mode: Local Mapping is deactivated
307 
308  if(mState==LOST)
309  {
310  bOK = Relocalization();
311  }
312  else
313  {
314  if(!mbVO)
315  {
316  // In last frame we tracked enough MapPoints in the map
317 
318  if(!mVelocity.empty())
319  {
320  bOK = TrackWithMotionModel();
321  }
322  else
323  {
324  bOK = TrackReferenceKeyFrame();
325  }
326  }
327  else
328  {
329  // In last frame we tracked mainly "visual odometry" points.
330 
331  // We compute two camera poses, one from motion model and one doing relocalization.
332  // If relocalization is sucessfull we choose that solution, otherwise we retain
333  // the "visual odometry" solution.
334 
335  bool bOKMM = false;
336  bool bOKReloc = false;
337  vector<MapPoint*> vpMPsMM;
338  vector<bool> vbOutMM;
339  cv::Mat TcwMM;
340  if(!mVelocity.empty())
341  {
342  bOKMM = TrackWithMotionModel();
343  vpMPsMM = mCurrentFrame.mvpMapPoints;
344  vbOutMM = mCurrentFrame.mvbOutlier;
345  TcwMM = mCurrentFrame.mTcw.clone();
346  }
347  bOKReloc = Relocalization();
348 
349  if(bOKMM && !bOKReloc)
350  {
351  mCurrentFrame.SetPose(TcwMM);
352  mCurrentFrame.mvpMapPoints = vpMPsMM;
353  mCurrentFrame.mvbOutlier = vbOutMM;
354 
355  if(mbVO)
356  {
357  for(int i =0; i<mCurrentFrame.N; i++)
358  {
360  {
361  mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
362  }
363  }
364  }
365  }
366  else if(bOKReloc)
367  {
368  mbVO = false;
369  }
370 
371  bOK = bOKReloc || bOKMM;
372  }
373  }
374  }
375 
377 
378  // If we have an initial estimation of the camera pose and matching. Track the local map.
379  if(!mbOnlyTracking)
380  {
381  if(bOK)
382  bOK = TrackLocalMap();
383  }
384  else
385  {
386  // mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
387  // a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
388  // the camera we will use the local map again.
389  if(bOK && !mbVO)
390  bOK = TrackLocalMap();
391  }
392 
393  if(bOK)
394  mState = OK;
395  else
396  mState=LOST;
397 
398  // Update drawer
399  mpFrameDrawer->Update(this);
400 
401  // If tracking were good, check if we insert a keyframe
402  if(bOK)
403  {
404  // Update motion model
405  if(!mLastFrame.mTcw.empty())
406  {
407  cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
408  mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
409  mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
410  mVelocity = mCurrentFrame.mTcw*LastTwc;
411  }
412  else
413  mVelocity = cv::Mat();
414 
415  //mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
416 
417  // Clean VO matches
418  for(int i=0; i<mCurrentFrame.N; i++)
419  {
421  if(pMP)
422  if(pMP->Observations()<1)
423  {
424  mCurrentFrame.mvbOutlier[i] = false;
425  mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
426  }
427  }
428 
429  // Delete temporal MapPoints
430  for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++)
431  {
432  MapPoint* pMP = *lit;
433  delete pMP;
434  }
435  mlpTemporalPoints.clear();
436 
437  // Check if we need to insert a new keyframe
438  if(NeedNewKeyFrame())
440 
441  // We allow points with high innovation (considererd outliers by the Huber Function)
442  // pass to the new keyframe, so that bundle adjustment will finally decide
443  // if they are outliers or not. We don't want next frame to estimate its position
444  // with those points so we discard them in the frame.
445  for(int i=0; i<mCurrentFrame.N;i++)
446  {
448  mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
449  }
450  }
451 
452  // Reset if the camera get lost soon after initialization
453  if(mState==LOST)
454  {
456  {
457  cout << "Track lost soon after initialisation, reseting..." << endl;
458  mpSystem->Reset();
459  return;
460  }
461  }
462 
465 
467  }
468 
469  // Store frame pose information to retrieve the complete camera trajectory afterwards.
470  if(!mCurrentFrame.mTcw.empty())
471  {
473  mlRelativeFramePoses.push_back(Tcr);
474  mlpReferences.push_back(mpReferenceKF);
476  mlbLost.push_back(mState==LOST);
477  }
478  else
479  {
480  // This can happen if tracking is lost
481  mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
482  mlpReferences.push_back(mlpReferences.back());
483  mlFrameTimes.push_back(mlFrameTimes.back());
484  mlbLost.push_back(mState==LOST);
485  }
486 
487 }
488 
489 
491 {
492  if(mCurrentFrame.N>500)
493  {
494  // Set Frame pose to the origin
495  mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
496 
497  // Create KeyFrame
499 
500  // Insert KeyFrame in the map
501  mpMap->AddKeyFrame(pKFini);
502 
503  // Create MapPoints and asscoiate to KeyFrame
504  for(int i=0; i<mCurrentFrame.N;i++)
505  {
506  float z = mCurrentFrame.mvDepth[i];
507  if(z>0)
508  {
509  cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
510  MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);
511  pNewMP->AddObservation(pKFini,i);
512  pKFini->AddMapPoint(pNewMP,i);
514  pNewMP->UpdateNormalAndDepth();
515  mpMap->AddMapPoint(pNewMP);
516 
517  mCurrentFrame.mvpMapPoints[i]=pNewMP;
518  }
519  }
520 
521  cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl;
522 
523  mpLocalMapper->InsertKeyFrame(pKFini);
524 
527  mpLastKeyFrame = pKFini;
528 
529  mvpLocalKeyFrames.push_back(pKFini);
531  mpReferenceKF = pKFini;
532  mCurrentFrame.mpReferenceKF = pKFini;
533 
535 
536  mpMap->mvpKeyFrameOrigins.push_back(pKFini);
537 
538  //mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
539 
540  mState=OK;
541  }
542 }
543 
545 {
546 
547  if(!mpInitializer)
548  {
549  // Set Reference Frame
550  if(mCurrentFrame.mvKeys.size()>100)
551  {
554  mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
555  for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
557 
558  if(mpInitializer)
559  delete mpInitializer;
560 
561  mpInitializer = new Initializer(mCurrentFrame,1.0,200);
562 
563  fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
564 
565  return;
566  }
567  }
568  else
569  {
570  // Try to initialize
571  if((int)mCurrentFrame.mvKeys.size()<=100)
572  {
573  delete mpInitializer;
574  mpInitializer = static_cast<Initializer*>(NULL);
575  fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
576  return;
577  }
578 
579  // Find correspondences
580  ORBmatcher matcher(0.9,true);
582 
583  // Check if there are enough correspondences
584  if(nmatches<100)
585  {
586  delete mpInitializer;
587  mpInitializer = static_cast<Initializer*>(NULL);
588  return;
589  }
590 
591  cv::Mat Rcw; // Current Camera Rotation
592  cv::Mat tcw; // Current Camera Translation
593  vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
594 
595  if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
596  {
597  for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
598  {
599  if(mvIniMatches[i]>=0 && !vbTriangulated[i])
600  {
601  mvIniMatches[i]=-1;
602  nmatches--;
603  }
604  }
605 
606  // Set Frame Poses
607  mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
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));
611  mCurrentFrame.SetPose(Tcw);
612 
614  }
615  }
616 }
617 
619 {
620  // Create KeyFrames
623 
624 
625  pKFini->ComputeBoW();
626  pKFcur->ComputeBoW();
627 
628  // Insert KFs in the map
629  mpMap->AddKeyFrame(pKFini);
630  mpMap->AddKeyFrame(pKFcur);
631 
632  // Create MapPoints and asscoiate to keyframes
633  for(size_t i=0; i<mvIniMatches.size();i++)
634  {
635  if(mvIniMatches[i]<0)
636  continue;
637 
638  //Create MapPoint.
639  cv::Mat worldPos(mvIniP3D[i]);
640 
641  MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);
642 
643  pKFini->AddMapPoint(pMP,i);
644  pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
645 
646  pMP->AddObservation(pKFini,i);
647  pMP->AddObservation(pKFcur,mvIniMatches[i]);
648 
650  pMP->UpdateNormalAndDepth();
651 
652  //Fill Current Frame structure
655 
656  //Add to Map
657  mpMap->AddMapPoint(pMP);
658  }
659 
660  // Update Connections
661  pKFini->UpdateConnections();
662  pKFcur->UpdateConnections();
663 
664  // Bundle Adjustment
665  cout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl;
666 
668 
669  // Set median depth to 1
670  float medianDepth = pKFini->ComputeSceneMedianDepth(2);
671  float invMedianDepth = 1.0f/medianDepth;
672 
673  if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
674  {
675  cout << "Wrong initialization, reseting..." << endl;
676  Reset();
677  return;
678  }
679 
680  // Scale initial baseline
681  cv::Mat Tc2w = pKFcur->GetPose();
682  Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
683  pKFcur->SetPose(Tc2w);
684 
685  // Scale points
686  vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches();
687  for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++)
688  {
689  if(vpAllMapPoints[iMP])
690  {
691  MapPoint* pMP = vpAllMapPoints[iMP];
692  pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth);
693  }
694  }
695 
696  mpLocalMapper->InsertKeyFrame(pKFini);
697  mpLocalMapper->InsertKeyFrame(pKFcur);
698 
699  mCurrentFrame.SetPose(pKFcur->GetPose());
701  mpLastKeyFrame = pKFcur;
702 
703  mvpLocalKeyFrames.push_back(pKFcur);
704  mvpLocalKeyFrames.push_back(pKFini);
706  mpReferenceKF = pKFcur;
707  mCurrentFrame.mpReferenceKF = pKFcur;
708 
710 
712 
713  //mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());
714 
715  mpMap->mvpKeyFrameOrigins.push_back(pKFini);
716 
717  mState=OK;
718 }
719 
721 {
722  for(int i =0; i<mLastFrame.N; i++)
723  {
724  MapPoint* pMP = mLastFrame.mvpMapPoints[i];
725 
726  if(pMP)
727  {
728  MapPoint* pRep = pMP->GetReplaced();
729  if(pRep)
730  {
731  mLastFrame.mvpMapPoints[i] = pRep;
732  }
733  }
734  }
735 }
736 
737 
739 {
740  // Compute Bag of Words vector
742 
743  // We perform first an ORB matching with the reference keyframe
744  // If enough matches are found we setup a PnP solver
745  ORBmatcher matcher(0.7,true);
746  vector<MapPoint*> vpMapPointMatches;
747 
748  int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
749 
750  if(nmatches<15)
751  return false;
752 
753  mCurrentFrame.mvpMapPoints = vpMapPointMatches;
755 
757 
758  // Discard outliers
759  int nmatchesMap = 0;
760  for(int i =0; i<mCurrentFrame.N; i++)
761  {
763  {
765  {
767 
768  mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
769  mCurrentFrame.mvbOutlier[i]=false;
770  pMP->mbTrackInView = false;
772  nmatches--;
773  }
774  else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
775  nmatchesMap++;
776  }
777  }
778 
779  return nmatchesMap>=10;
780 }
781 
783 {
784  // Update pose according to reference keyframe
786  cv::Mat Tlr = mlRelativeFramePoses.back();
787 
788  mLastFrame.SetPose(Tlr*pRef->GetPose());
789 
791  return;
792 
793  // Create "visual odometry" MapPoints
794  // We sort points according to their measured depth by the stereo/RGB-D sensor
795  vector<pair<float,int> > vDepthIdx;
796  vDepthIdx.reserve(mLastFrame.N);
797  for(int i=0; i<mLastFrame.N;i++)
798  {
799  float z = mLastFrame.mvDepth[i];
800  if(z>0)
801  {
802  vDepthIdx.push_back(make_pair(z,i));
803  }
804  }
805 
806  if(vDepthIdx.empty())
807  return;
808 
809  sort(vDepthIdx.begin(),vDepthIdx.end());
810 
811  // We insert all close points (depth<mThDepth)
812  // If less than 100 close points, we insert the 100 closest ones.
813  int nPoints = 0;
814  for(size_t j=0; j<vDepthIdx.size();j++)
815  {
816  int i = vDepthIdx[j].second;
817 
818  bool bCreateNew = false;
819 
820  MapPoint* pMP = mLastFrame.mvpMapPoints[i];
821  if(!pMP)
822  bCreateNew = true;
823  else if(pMP->Observations()<1)
824  {
825  bCreateNew = true;
826  }
827 
828  if(bCreateNew)
829  {
830  cv::Mat x3D = mLastFrame.UnprojectStereo(i);
831  MapPoint* pNewMP = new MapPoint(x3D,mpMap,&mLastFrame,i);
832 
833  mLastFrame.mvpMapPoints[i]=pNewMP;
834 
835  mlpTemporalPoints.push_back(pNewMP);
836  nPoints++;
837  }
838  else
839  {
840  nPoints++;
841  }
842 
843  if(vDepthIdx[j].first>mThDepth && nPoints>100)
844  break;
845  }
846 }
847 
849 {
850  ORBmatcher matcher(0.9,true);
851 
852  // Update last frame pose according to its reference keyframe
853  // Create "visual odometry" points if in Localization Mode
854  UpdateLastFrame();
855 
857 
858  fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
859 
860  // Project points seen in previous frame
861  int th;
863  th=15;
864  else
865  th=7;
867 
868  // If few matches, uses a wider window search
869  if(nmatches<20)
870  {
871  fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
873  }
874 
875  if(nmatches<20)
876  return false;
877 
878  // Optimize frame pose with all matches
880 
881  // Discard outliers
882  int nmatchesMap = 0;
883  for(int i =0; i<mCurrentFrame.N; i++)
884  {
886  {
888  {
890 
891  mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
892  mCurrentFrame.mvbOutlier[i]=false;
893  pMP->mbTrackInView = false;
895  nmatches--;
896  }
897  else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
898  nmatchesMap++;
899  }
900  }
901 
902  if(mbOnlyTracking)
903  {
904  mbVO = nmatchesMap<10;
905  return nmatches>20;
906  }
907 
908  return nmatchesMap>=10;
909 }
910 
912 {
913  // We have an estimation of the camera pose and some map points tracked in the frame.
914  // We retrieve the local map and try to find matches to points in the local map.
915 
916  UpdateLocalMap();
917 
919 
920  // Optimize Pose
922  mnMatchesInliers = 0;
923 
924  // Update MapPoints Statistics
925  for(int i=0; i<mCurrentFrame.N; i++)
926  {
928  {
929  if(!mCurrentFrame.mvbOutlier[i])
930  {
931  mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
932  if(!mbOnlyTracking)
933  {
934  if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
936  }
937  else
939  }
940  else if(mSensor==System::STEREO)
941  mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
942 
943  }
944  }
945 
946  // Decide if the tracking was succesful
947  // More restrictive if there was a relocalization recently
949  return false;
950 
951  if(mnMatchesInliers<30)
952  return false;
953  else
954  return true;
955 }
956 
957 
959 {
960  if(mbOnlyTracking)
961  return false;
962 
963  // If Local Mapping is freezed by a Loop Closure do not insert keyframes
965  return false;
966 
967  const int nKFs = mpMap->KeyFramesInMap();
968 
969  // Do not insert keyframes if not enough frames have passed from last relocalisation
971  return false;
972 
973  // Tracked MapPoints in the reference keyframe
974  int nMinObs = 3;
975  if(nKFs<=2)
976  nMinObs=2;
977  int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);
978 
979  // Local Mapping accept keyframes?
980  bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();
981 
982  // Check how many "close" points are being tracked and how many could be potentially created.
983  int nNonTrackedClose = 0;
984  int nTrackedClose= 0;
986  {
987  for(int i =0; i<mCurrentFrame.N; i++)
988  {
990  {
992  nTrackedClose++;
993  else
994  nNonTrackedClose++;
995  }
996  }
997  }
998 
999  bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);
1000 
1001  // Thresholds
1002  float thRefRatio = 0.75f;
1003  if(nKFs<2)
1004  thRefRatio = 0.4f;
1005 
1007  thRefRatio = 0.9f;
1008 
1009  // Condition 1a: More than "MaxFrames" have passed from last keyframe insertion
1010  const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;
1011  // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
1012  const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);
1013  //Condition 1c: tracking is weak
1014  const bool c1c = mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || bNeedToInsertClose) ;
1015  // Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
1016  const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);
1017 
1018  if((c1a||c1b||c1c)&&c2)
1019  {
1020  // If the mapping accepts keyframes, insert keyframe.
1021  // Otherwise send a signal to interrupt BA
1022  if(bLocalMappingIdle)
1023  {
1024  return true;
1025  }
1026  else
1027  {
1030  {
1032  return true;
1033  else
1034  return false;
1035  }
1036  else
1037  return false;
1038  }
1039  }
1040  else
1041  return false;
1042 }
1043 
1045 {
1046  if(!mpLocalMapper->SetNotStop(true))
1047  return;
1048 
1050 
1051  mpReferenceKF = pKF;
1053 
1055  {
1057 
1058  // We sort points by the measured depth by the stereo/RGBD sensor.
1059  // We create all those MapPoints whose depth < mThDepth.
1060  // If there are less than 100 close points we create the 100 closest.
1061  vector<pair<float,int> > vDepthIdx;
1062  vDepthIdx.reserve(mCurrentFrame.N);
1063  for(int i=0; i<mCurrentFrame.N; i++)
1064  {
1065  float z = mCurrentFrame.mvDepth[i];
1066  if(z>0)
1067  {
1068  vDepthIdx.push_back(make_pair(z,i));
1069  }
1070  }
1071 
1072  if(!vDepthIdx.empty())
1073  {
1074  sort(vDepthIdx.begin(),vDepthIdx.end());
1075 
1076  int nPoints = 0;
1077  for(size_t j=0; j<vDepthIdx.size();j++)
1078  {
1079  int i = vDepthIdx[j].second;
1080 
1081  bool bCreateNew = false;
1082 
1084  if(!pMP)
1085  bCreateNew = true;
1086  else if(pMP->Observations()<1)
1087  {
1088  bCreateNew = true;
1089  mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
1090  }
1091 
1092  if(bCreateNew)
1093  {
1094  cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
1095  MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);
1096  pNewMP->AddObservation(pKF,i);
1097  pKF->AddMapPoint(pNewMP,i);
1099  pNewMP->UpdateNormalAndDepth();
1100  mpMap->AddMapPoint(pNewMP);
1101 
1102  mCurrentFrame.mvpMapPoints[i]=pNewMP;
1103  nPoints++;
1104  }
1105  else
1106  {
1107  nPoints++;
1108  }
1109 
1110  if(vDepthIdx[j].first>mThDepth && nPoints>100)
1111  break;
1112  }
1113  }
1114  }
1115 
1117 
1118  mpLocalMapper->SetNotStop(false);
1119 
1121  mpLastKeyFrame = pKF;
1122 }
1123 
1125 {
1126  // Do not search map points already matched
1127  for(vector<MapPoint*>::iterator vit=mCurrentFrame.mvpMapPoints.begin(), vend=mCurrentFrame.mvpMapPoints.end(); vit!=vend; vit++)
1128  {
1129  MapPoint* pMP = *vit;
1130  if(pMP)
1131  {
1132  if(pMP->isBad())
1133  {
1134  *vit = static_cast<MapPoint*>(NULL);
1135  }
1136  else
1137  {
1138  pMP->IncreaseVisible();
1140  pMP->mbTrackInView = false;
1141  }
1142  }
1143  }
1144 
1145  int nToMatch=0;
1146 
1147  // Project points in frame and check its visibility
1148  for(vector<MapPoint*>::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++)
1149  {
1150  MapPoint* pMP = *vit;
1151  if(pMP->mnLastFrameSeen == mCurrentFrame.mnId)
1152  continue;
1153  if(pMP->isBad())
1154  continue;
1155  // Project (this fills MapPoint variables for matching)
1156  if(mCurrentFrame.isInFrustum(pMP,0.5))
1157  {
1158  pMP->IncreaseVisible();
1159  nToMatch++;
1160  }
1161  }
1162 
1163  if(nToMatch>0)
1164  {
1165  ORBmatcher matcher(0.8);
1166  int th = 1;
1167  if(mSensor==System::RGBD)
1168  th=3;
1169  // If the camera has been relocalised recently, perform a coarser search
1171  th=5;
1173  }
1174 }
1175 
1177 {
1178  // This is for visualization
1180 
1181  // Update
1184 }
1185 
1187 {
1188  mvpLocalMapPoints.clear();
1189 
1190  for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
1191  {
1192  KeyFrame* pKF = *itKF;
1193  const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();
1194 
1195  for(vector<MapPoint*>::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++)
1196  {
1197  MapPoint* pMP = *itMP;
1198  if(!pMP)
1199  continue;
1201  continue;
1202  if(!pMP->isBad())
1203  {
1204  mvpLocalMapPoints.push_back(pMP);
1206  }
1207  }
1208  }
1209 }
1210 
1211 
1213 {
1214  // Each map point vote for the keyframes in which it has been observed
1215  map<KeyFrame*,int> keyframeCounter;
1216  for(int i=0; i<mCurrentFrame.N; i++)
1217  {
1219  {
1221  if(!pMP->isBad())
1222  {
1223  const map<KeyFrame*,size_t> observations = pMP->GetObservations();
1224  for(map<KeyFrame*,size_t>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++)
1225  keyframeCounter[it->first]++;
1226  }
1227  else
1228  {
1229  mCurrentFrame.mvpMapPoints[i]=NULL;
1230  }
1231  }
1232  }
1233 
1234  if(keyframeCounter.empty())
1235  return;
1236 
1237  int max=0;
1238  KeyFrame* pKFmax= static_cast<KeyFrame*>(NULL);
1239 
1240  mvpLocalKeyFrames.clear();
1241  mvpLocalKeyFrames.reserve(3*keyframeCounter.size());
1242 
1243  // All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points
1244  for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++)
1245  {
1246  KeyFrame* pKF = it->first;
1247 
1248  if(pKF->isBad())
1249  continue;
1250 
1251  if(it->second>max)
1252  {
1253  max=it->second;
1254  pKFmax=pKF;
1255  }
1256 
1257  mvpLocalKeyFrames.push_back(it->first);
1259  }
1260 
1261 
1262  // Include also some not-already-included keyframes that are neighbors to already-included keyframes
1263  for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
1264  {
1265  // Limit the number of keyframes
1266  if(mvpLocalKeyFrames.size()>80)
1267  break;
1268 
1269  KeyFrame* pKF = *itKF;
1270 
1271  const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
1272 
1273  for(vector<KeyFrame*>::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++)
1274  {
1275  KeyFrame* pNeighKF = *itNeighKF;
1276  if(!pNeighKF->isBad())
1277  {
1279  {
1280  mvpLocalKeyFrames.push_back(pNeighKF);
1282  break;
1283  }
1284  }
1285  }
1286 
1287  const set<KeyFrame*> spChilds = pKF->GetChilds();
1288  for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++)
1289  {
1290  KeyFrame* pChildKF = *sit;
1291  if(!pChildKF->isBad())
1292  {
1294  {
1295  mvpLocalKeyFrames.push_back(pChildKF);
1297  break;
1298  }
1299  }
1300  }
1301 
1302  KeyFrame* pParent = pKF->GetParent();
1303  if(pParent)
1304  {
1306  {
1307  mvpLocalKeyFrames.push_back(pParent);
1309  break;
1310  }
1311  }
1312 
1313  }
1314 
1315  if(pKFmax)
1316  {
1317  mpReferenceKF = pKFmax;
1319  }
1320 }
1321 
1323 {
1324  // Compute Bag of Words Vector
1326 
1327  // Relocalization is performed when tracking is lost
1328  // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
1329  vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
1330 
1331  if(vpCandidateKFs.empty())
1332  return false;
1333 
1334  const int nKFs = vpCandidateKFs.size();
1335 
1336  // We perform first an ORB matching with each candidate
1337  // If enough matches are found we setup a PnP solver
1338  ORBmatcher matcher(0.75,true);
1339 
1340  vector<PnPsolver*> vpPnPsolvers;
1341  vpPnPsolvers.resize(nKFs);
1342 
1343  vector<vector<MapPoint*> > vvpMapPointMatches;
1344  vvpMapPointMatches.resize(nKFs);
1345 
1346  vector<bool> vbDiscarded;
1347  vbDiscarded.resize(nKFs);
1348 
1349  int nCandidates=0;
1350 
1351  for(int i=0; i<nKFs; i++)
1352  {
1353  KeyFrame* pKF = vpCandidateKFs[i];
1354  if(pKF->isBad())
1355  vbDiscarded[i] = true;
1356  else
1357  {
1358  int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
1359  if(nmatches<15)
1360  {
1361  vbDiscarded[i] = true;
1362  continue;
1363  }
1364  else
1365  {
1366  PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
1367  pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);
1368  vpPnPsolvers[i] = pSolver;
1369  nCandidates++;
1370  }
1371  }
1372  }
1373 
1374  // Alternatively perform some iterations of P4P RANSAC
1375  // Until we found a camera pose supported by enough inliers
1376  bool bMatch = false;
1377  ORBmatcher matcher2(0.9,true);
1378 
1379  while(nCandidates>0 && !bMatch)
1380  {
1381  for(int i=0; i<nKFs; i++)
1382  {
1383  if(vbDiscarded[i])
1384  continue;
1385 
1386  // Perform 5 Ransac Iterations
1387  vector<bool> vbInliers;
1388  int nInliers;
1389  bool bNoMore;
1390 
1391  PnPsolver* pSolver = vpPnPsolvers[i];
1392  cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
1393 
1394  // If Ransac reachs max. iterations discard keyframe
1395  if(bNoMore)
1396  {
1397  vbDiscarded[i]=true;
1398  nCandidates--;
1399  }
1400 
1401  // If a Camera Pose is computed, optimize
1402  if(!Tcw.empty())
1403  {
1404  Tcw.copyTo(mCurrentFrame.mTcw);
1405 
1406  set<MapPoint*> sFound;
1407 
1408  const int np = vbInliers.size();
1409 
1410  for(int j=0; j<np; j++)
1411  {
1412  if(vbInliers[j])
1413  {
1414  mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];
1415  sFound.insert(vvpMapPointMatches[i][j]);
1416  }
1417  else
1418  mCurrentFrame.mvpMapPoints[j]=NULL;
1419  }
1420 
1422 
1423  if(nGood<10)
1424  continue;
1425 
1426  for(int io =0; io<mCurrentFrame.N; io++)
1427  if(mCurrentFrame.mvbOutlier[io])
1428  mCurrentFrame.mvpMapPoints[io]=static_cast<MapPoint*>(NULL);
1429 
1430  // If few inliers, search by projection in a coarse window and optimize again
1431  if(nGood<50)
1432  {
1433  int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);
1434 
1435  if(nadditional+nGood>=50)
1436  {
1438 
1439  // If many inliers but still not enough, search by projection again in a narrower window
1440  // the camera has been already optimized with many points
1441  if(nGood>30 && nGood<50)
1442  {
1443  sFound.clear();
1444  for(int ip =0; ip<mCurrentFrame.N; ip++)
1445  if(mCurrentFrame.mvpMapPoints[ip])
1446  sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
1447  nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,3,64);
1448 
1449  // Final optimization
1450  if(nGood+nadditional>=50)
1451  {
1453 
1454  for(int io =0; io<mCurrentFrame.N; io++)
1455  if(mCurrentFrame.mvbOutlier[io])
1456  mCurrentFrame.mvpMapPoints[io]=NULL;
1457  }
1458  }
1459  }
1460  }
1461 
1462 
1463  // If the pose is supported by enough inliers stop ransacs and continue
1464  if(nGood>=50)
1465  {
1466  bMatch = true;
1467  break;
1468  }
1469  }
1470  }
1471  }
1472 
1473  if(!bMatch)
1474  {
1475  mCurrentFrame.mTcw = cv::Mat::zeros(0, 0, CV_32F); // this prevents a segfault later (https://github.com/raulmur/ORB_SLAM2/pull/381#issuecomment-337312336)
1476  return false;
1477  }
1478  else
1479  {
1481  return true;
1482  }
1483 
1484 }
1485 
1487 {
1488 
1489  cout << "System Reseting" << endl;
1490 
1491  // Reset Local Mapping
1492  cout << "Reseting Local Mapper...";
1494  cout << " done" << endl;
1495 
1496  // Reset Loop Closing
1497  cout << "Reseting Loop Closing...";
1499  cout << " done" << endl;
1500 
1501  // Clear BoW Database
1502  cout << "Reseting Database...";
1503  mpKeyFrameDB->clear();
1504  cout << " done" << endl;
1505 
1506  // Clear Map (this erase MapPoints and KeyFrames)
1507  mpMap->clear();
1508 
1509  KeyFrame::nNextId = 0;
1510  Frame::nNextId = 0;
1512 
1513  if(mpInitializer)
1514  {
1515  delete mpInitializer;
1516  mpInitializer = static_cast<Initializer*>(NULL);
1517  }
1518 
1519  mlRelativeFramePoses.clear();
1520  mlpReferences.clear();
1521  mlFrameTimes.clear();
1522  mlbLost.clear();
1523 
1524 }
1525 
1526 void Tracking::ChangeCalibration(const string &strSettingPath)
1527 {
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"];
1533 
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;
1539  K.copyTo(mK);
1540 
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"];
1547  if(k3!=0)
1548  {
1549  DistCoef.resize(5);
1550  DistCoef.at<float>(4) = k3;
1551  }
1552  DistCoef.copyTo(mDistCoef);
1553 
1554  mbf = fSettings["Camera.bf"];
1555 
1557 }
1558 
1559 void Tracking::InformOnlyTracking(const bool &flag)
1560 {
1561  mbOnlyTracking = flag;
1562 }
1563 
1564 
1565 
1566 } //namespace ORB_SLAM
std::vector< KeyFrame * > GetBestCovisibilityKeyFrames(const int &N)
Definition: KeyFrame.cc:174
std::vector< MapPoint * > mvpLocalMapPoints
Definition: Tracking.h:186
cv::Mat GetPoseInverse()
Definition: KeyFrame.cc:92
std::vector< KeyFrame * > mvpLocalKeyFrames
Definition: Tracking.h:185
unsigned int mnLastKeyFrameId
Definition: Tracking.h:220
void UpdateLocalPoints()
Definition: Tracking.cc:1186
std::vector< MapPoint * > mvpMapPoints
Definition: Frame.h:153
unsigned int mnLastRelocFrameId
Definition: Tracking.h:221
void ComputeBoW()
Definition: Frame.cc:395
std::vector< KeyFrame * > DetectRelocalizationCandidates(Frame *F)
void CreateNewKeyFrame()
Definition: Tracking.cc:1044
long unsigned int mnTrackReferenceForFrame
Definition: KeyFrame.h:137
f
void SetLoopClosing(LoopClosing *pLoopClosing)
Definition: Tracking.cc:142
int SearchByBoW(KeyFrame *pKF, Frame &F, std::vector< MapPoint * > &vpMapPointMatches)
KeyFrame * mpLastKeyFrame
Definition: Tracking.h:218
bool TrackWithMotionModel()
Definition: Tracking.cc:848
cv::Mat GetPose()
Definition: KeyFrame.cc:86
std::vector< int > mvIniMatches
Definition: Tracking.h:113
void AddMapPoint(MapPoint *pMP, const size_t &idx)
Definition: KeyFrame.cc:210
void SetLocalMapper(LocalMapping *pLocalMapper)
Definition: Tracking.cc:137
cv::Mat GrabImageRGBD(const cv::Mat &imRGB, const cv::Mat &imD, const double &timestamp)
Definition: Tracking.cc:188
void UpdateLastFrame()
Definition: Tracking.cc:782
std::mutex mMutexMapUpdate
Definition: Map.h:65
bool NeedNewKeyFrame()
Definition: Tracking.cc:958
cv::Mat UnprojectStereo(const int &i)
Definition: Frame.cc:666
Initializer * mpInitializer
Definition: Tracking.h:181
cv::Mat GetWorldPos()
Definition: MapPoint.cc:80
std::vector< MapPoint * > GetMapPointMatches()
Definition: KeyFrame.cc:277
std::vector< bool > mvbOutlier
Definition: Frame.h:156
void IncreaseVisible(int n=1)
Definition: MapPoint.cc:224
list< bool > mlbLost
Definition: Tracking.h:123
bool SetNotStop(bool flag)
cv::Mat GetRotationInverse()
Definition: Frame.h:78
void UpdatePoseMatrices()
Definition: Frame.cc:261
FrameDrawer * mpFrameDrawer
Definition: Tracking.h:192
cv::Mat iterate(int nIterations, bool &bNoMore, vector< bool > &vbInliers, int &nInliers)
Definition: PnPsolver.cc:165
void AddObservation(KeyFrame *pKF, size_t idx)
Definition: MapPoint.cc:98
ORBVocabulary * mpORBVocabulary
Definition: Tracking.h:177
System * mpSystem
Definition: Tracking.h:189
int TrackedMapPoints(const int &minObs)
Definition: KeyFrame.cc:250
KeyFrameDatabase * mpKeyFrameDB
Definition: Tracking.h:178
cv::Mat GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double &timestamp)
Definition: Tracking.cc:148
void UpdateNormalAndDepth()
Definition: MapPoint.cc:330
void AddMapPoint(MapPoint *pMP)
Definition: Map.cc:40
std::vector< cv::KeyPoint > mvKeysUn
Definition: Frame.h:138
std::vector< MapPoint * > GetAllMapPoints()
Definition: Map.cc:88
KeyFrame * mpReferenceKF
Definition: Frame.h:171
cv::Mat GetCameraCenter()
Definition: Frame.h:73
vector< KeyFrame * > mvpKeyFrameOrigins
Definition: Map.h:63
void MonocularInitialization()
Definition: Tracking.cc:544
static void GlobalBundleAdjustemnt(Map *pMap, int nIterations=5, bool *pbStopFlag=NULL, const unsigned long nLoopKF=0, const bool bRobust=true)
Definition: Optimizer.cc:41
eTrackingState mState
Definition: Tracking.h:101
std::map< KeyFrame *, size_t > GetObservations()
Definition: MapPoint.cc:139
void InsertKeyFrame(KeyFrame *pKF)
bool isInFrustum(MapPoint *pMP, float viewingCosLimit)
Definition: Frame.cc:269
void CreateInitialMapMonocular()
Definition: Tracking.cc:618
std::vector< float > mvDepth
Definition: Frame.h:143
long unsigned KeyFramesInMap()
Definition: Map.cc:100
static bool mbInitialComputations
Definition: Frame.h:188
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)
void ChangeCalibration(const string &strSettingPath)
Definition: Tracking.cc:1526
LoopClosing * mpLoopClosing
Definition: Tracking.h:170
cv::Mat mTcw
Definition: Frame.h:164
list< double > mlFrameTimes
Definition: Tracking.h:122
static long unsigned int nNextId
Definition: Frame.h:167
KeyFrame * mpReferenceKF
Definition: Tracking.h:184
void SetWorldPos(const cv::Mat &Pos)
Definition: MapPoint.cc:73
void clear()
Definition: Map.cc:118
double mTimeStamp
Definition: Frame.h:109
long unsigned int MapPointsInMap()
Definition: Map.cc:94
ORBextractor * mpORBextractorLeft
Definition: Tracking.h:173
MapPoint * GetReplaced()
Definition: MapPoint.cc:170
int SearchByProjection(Frame &F, const std::vector< MapPoint * > &vpMapPoints, const float th=3)
std::vector< cv::KeyPoint > mvKeys
Definition: Frame.h:137
static int PoseOptimization(Frame *pFrame)
Definition: Optimizer.cc:239
void InformOnlyTracking(const bool &flag)
Definition: Tracking.cc:1559
int SearchForInitialization(Frame &F1, Frame &F2, std::vector< cv::Point2f > &vbPrevMatched, std::vector< int > &vnMatches12, int windowSize=10)
Definition: ORBmatcher.cc:405
float ComputeSceneMedianDepth(const int q)
Definition: KeyFrame.cc:633
long unsigned int mnLastFrameSeen
Definition: MapPoint.h:100
void SetPose(const cv::Mat &Tcw)
Definition: KeyFrame.cc:70
TFSIMD_FORCE_INLINE const tfScalar & z() const
ORBextractor * mpIniORBextractor
Definition: Tracking.h:174
void SetRansacParameters(double probability=0.99, int minInliers=8, int maxIterations=300, int minSet=4, float epsilon=0.4, float th2=5.991)
Definition: PnPsolver.cc:121
list< KeyFrame * > mlpReferences
Definition: Tracking.h:121
void CheckReplacedInLastFrame()
Definition: Tracking.cc:720
void StereoInitialization()
Definition: Tracking.cc:490
eTrackingState mLastProcessedState
Definition: Tracking.h:102
void Update(Tracking *pTracker)
Definition: FrameDrawer.cc:167
std::set< KeyFrame * > GetChilds()
Definition: KeyFrame.cc:400
void SearchLocalPoints()
Definition: Tracking.cc:1124
bool TrackReferenceKeyFrame()
Definition: Tracking.cc:738
static long unsigned int nNextId
Definition: KeyFrame.h:124
void SetPose(cv::Mat Tcw)
Definition: Frame.cc:255
void SetReferenceMapPoints(const std::vector< MapPoint * > &vpMPs)
Definition: Map.cc:64
void AddKeyFrame(KeyFrame *pKF)
Definition: Map.cc:32
long unsigned int mnId
Definition: Frame.h:168
list< MapPoint * > mlpTemporalPoints
Definition: Tracking.h:229
bool Initialize(const Frame &CurrentFrame, const vector< int > &vMatches12, cv::Mat &R21, cv::Mat &t21, vector< cv::Point3f > &vP3D, vector< bool > &vbTriangulated)
Definition: Initializer.cc:44
long unsigned int mnTrackReferenceForFrame
Definition: MapPoint.h:99
list< cv::Mat > mlRelativeFramePoses
Definition: Tracking.h:120
cv::Mat GrabImageMonocular(const cv::Mat &im, const double &timestamp)
Definition: Tracking.cc:219
void UpdateLocalKeyFrames()
Definition: Tracking.cc:1212
LocalMapping * mpLocalMapper
Definition: Tracking.h:169
std::vector< cv::Point3f > mvIniP3D
Definition: Tracking.h:115
void ComputeDistinctiveDescriptors()
Definition: MapPoint.cc:242
std::vector< cv::Point2f > mvbPrevMatched
Definition: Tracking.h:114
void UpdateConnections()
Definition: KeyFrame.cc:289
KeyFrame * GetParent()
Definition: KeyFrame.cc:406
ORBextractor * mpORBextractorRight
Definition: Tracking.h:173


orb_slam2_ros
Author(s):
autogenerated on Wed Apr 21 2021 02:53:05