forked from raulmur/ORB_SLAM2
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Sim3Solver.h
133 lines (96 loc) · 3.27 KB
/
Sim3Solver.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
/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 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-SLAM2 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-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef SIM3SOLVER_H
#define SIM3SOLVER_H
#include <opencv2/opencv.hpp>
#include <vector>
#include "KeyFrame.h"
namespace ORB_SLAM2
{
class Sim3Solver
{
public:
Sim3Solver(KeyFrame* pKF1, KeyFrame* pKF2, const std::vector<MapPoint*> &vpMatched12, const bool bFixScale = true);
void SetRansacParameters(double probability = 0.99, int minInliers = 6 , int maxIterations = 300);
cv::Mat find(std::vector<bool> &vbInliers12, int &nInliers);
cv::Mat iterate(int nIterations, bool &bNoMore, std::vector<bool> &vbInliers, int &nInliers);
cv::Mat GetEstimatedRotation();
cv::Mat GetEstimatedTranslation();
float GetEstimatedScale();
protected:
void ComputeCentroid(cv::Mat &P, cv::Mat &Pr, cv::Mat &C);
void ComputeSim3(cv::Mat &P1, cv::Mat &P2);
void CheckInliers();
void Project(const std::vector<cv::Mat> &vP3Dw, std::vector<cv::Mat> &vP2D, cv::Mat Tcw, cv::Mat K);
void FromCameraToImage(const std::vector<cv::Mat> &vP3Dc, std::vector<cv::Mat> &vP2D, cv::Mat K);
protected:
// KeyFrames and matches
KeyFrame* mpKF1;
KeyFrame* mpKF2;
std::vector<cv::Mat> mvX3Dc1;
std::vector<cv::Mat> mvX3Dc2;
std::vector<MapPoint*> mvpMapPoints1;
std::vector<MapPoint*> mvpMapPoints2;
std::vector<MapPoint*> mvpMatches12;
std::vector<size_t> mvnIndices1;
std::vector<size_t> mvSigmaSquare1;
std::vector<size_t> mvSigmaSquare2;
std::vector<size_t> mvnMaxError1;
std::vector<size_t> mvnMaxError2;
int N;
int mN1;
// Current Estimation
cv::Mat mR12i;
cv::Mat mt12i;
float ms12i;
cv::Mat mT12i;
cv::Mat mT21i;
std::vector<bool> mvbInliersi;
int mnInliersi;
// Current Ransac State
int mnIterations;
std::vector<bool> mvbBestInliers;
int mnBestInliers;
cv::Mat mBestT12;
cv::Mat mBestRotation;
cv::Mat mBestTranslation;
float mBestScale;
// Scale is fixed to 1 in the stereo/RGBD case
bool mbFixScale;
// Indices for random selection
std::vector<size_t> mvAllIndices;
// Projections
std::vector<cv::Mat> mvP1im1;
std::vector<cv::Mat> mvP2im2;
// RANSAC probability
double mRansacProb;
// RANSAC min inliers
int mRansacMinInliers;
// RANSAC max iterations
int mRansacMaxIts;
// Threshold inlier/outlier. e = dist(Pi,T_ij*Pj)^2 < 5.991*mSigma2
float mTh;
float mSigma2;
// Calibration
cv::Mat mK1;
cv::Mat mK2;
};
} //namespace ORB_SLAM
#endif // SIM3SOLVER_H