Initializer.h
Go to the documentation of this file.
1 
20 #ifndef INITIALIZER_H
21 #define INITIALIZER_H
22 
23 #include<opencv2/opencv.hpp>
24 #include "Frame.h"
25 
26 
27 namespace ORB_SLAM2
28 {
29 
30 // THIS IS THE INITIALIZER FOR MONOCULAR SLAM. NOT USED IN THE STEREO OR RGBD CASE.
32 {
33  typedef pair<int,int> Match;
34 
35 public:
36 
37  // Fix the reference frame
38  Initializer(const Frame &ReferenceFrame, float sigma = 1.0, int iterations = 200);
39 
40  // Computes in parallel a fundamental matrix and a homography
41  // Selects a model and tries to recover the motion and the structure from motion
42  bool Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12,
43  cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated);
44 
45 
46 private:
47 
48  void FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21);
49  void FindFundamental(vector<bool> &vbInliers, float &score, cv::Mat &F21);
50 
51  cv::Mat ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2);
52  cv::Mat ComputeF21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2);
53 
54  float CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma);
55 
56  float CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma);
57 
58  bool ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,
59  cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated);
60 
61  bool ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
62  cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated);
63 
64  void Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D);
65 
66  void Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T);
67 
68  int CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2,
69  const vector<Match> &vMatches12, vector<bool> &vbInliers,
70  const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float &parallax);
71 
72  void DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t);
73 
74 
75  // Keypoints from Reference Frame (Frame 1)
76  vector<cv::KeyPoint> mvKeys1;
77 
78  // Keypoints from Current Frame (Frame 2)
79  vector<cv::KeyPoint> mvKeys2;
80 
81  // Current Matches from Reference to Current
82  vector<Match> mvMatches12;
83  vector<bool> mvbMatched1;
84 
85  // Calibration
86  cv::Mat mK;
87 
88  // Standard Deviation and Variance
89  float mSigma, mSigma2;
90 
91  // Ransac max iterations
93 
94  // Ransac sets
95  vector<vector<size_t> > mvSets;
96 
97 };
98 
99 } //namespace ORB_SLAM
100 
101 #endif // INITIALIZER_H
void FindFundamental(vector< bool > &vbInliers, float &score, cv::Mat &F21)
Definition: Initializer.cc:175
void DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
Definition: Initializer.cc:909
vector< cv::KeyPoint > mvKeys1
Definition: Initializer.h:76
cv::Mat ComputeF21(const vector< cv::Point2f > &vP1, const vector< cv::Point2f > &vP2)
Definition: Initializer.cc:268
int CheckRT(const cv::Mat &R, const cv::Mat &t, const vector< cv::KeyPoint > &vKeys1, const vector< cv::KeyPoint > &vKeys2, const vector< Match > &vMatches12, vector< bool > &vbInliers, const cv::Mat &K, vector< cv::Point3f > &vP3D, float th2, vector< bool > &vbGood, float &parallax)
Definition: Initializer.cc:798
float CheckFundamental(const cv::Mat &F21, vector< bool > &vbMatchesInliers, float sigma)
Definition: Initializer.cc:390
vector< vector< size_t > > mvSets
Definition: Initializer.h:95
void Normalize(const vector< cv::KeyPoint > &vKeys, vector< cv::Point2f > &vNormalizedPoints, cv::Mat &T)
Definition: Initializer.cc:749
void Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
Definition: Initializer.cc:734
pair< int, int > Match
Definition: Initializer.h:33
float CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector< bool > &vbMatchesInliers, float sigma)
Definition: Initializer.cc:305
Initializer(const Frame &ReferenceFrame, float sigma=1.0, int iterations=200)
Definition: Initializer.cc:33
vector< bool > mvbMatched1
Definition: Initializer.h:83
bool ReconstructH(vector< bool > &vbMatchesInliers, cv::Mat &H21, cv::Mat &K, cv::Mat &R21, cv::Mat &t21, vector< cv::Point3f > &vP3D, vector< bool > &vbTriangulated, float minParallax, int minTriangulated)
Definition: Initializer.cc:572
cv::Mat ComputeH21(const vector< cv::Point2f > &vP1, const vector< cv::Point2f > &vP2)
Definition: Initializer.cc:226
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
vector< cv::KeyPoint > mvKeys2
Definition: Initializer.h:79
vector< Match > mvMatches12
Definition: Initializer.h:82
bool ReconstructF(vector< bool > &vbMatchesInliers, cv::Mat &F21, cv::Mat &K, cv::Mat &R21, cv::Mat &t21, vector< cv::Point3f > &vP3D, vector< bool > &vbTriangulated, float minParallax, int minTriangulated)
Definition: Initializer.cc:470
void FindHomography(vector< bool > &vbMatchesInliers, float &score, cv::Mat &H21)
Definition: Initializer.cc:124


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