在Tracking模块里,单目第一步流程为MonocularInitialization,而这个函数主要涉及到的是Initializer.cc。
// THIS IS THE INITIALIZER FOR MONOCULAR SLAM. NOT USED IN THE STEREO OR RGBD CASE.
/**
* @brief 单目SLAM初始化相关,双目和RGBD不会使用这个类
*/
class Initializer
{
typedef pair Match;
public:
// Fix the reference frame
// 用reference frame来初始化,这个reference frame就是SLAM正式开始的第一帧
Initializer(const Frame &ReferenceFrame, float sigma = 1.0, int iterations = 200);
// Computes in parallel a fundamental matrix and a homography
// Selects a model and tries to recover the motion and the structure from motion
// 用current frame,也就是用SLAM逻辑上的第二帧来初始化整个SLAM,得到最开始两帧之间的R t,以及点云
bool Initialize(const Frame &CurrentFrame, const vector &vMatches12,
cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated);
private:
// 假设场景为平面情况下通过前两帧求取Homography矩阵(current frame 2 到 reference frame 1),并得到该模型的评分
void FindHomography(vector &vbMatchesInliers, float &score, cv::Mat &H21);
// 假设场景为非平面情况下通过前两帧求取Fundamental矩阵(current frame 2 到 reference frame 1),并得到该模型的评分
void FindFundamental(vector &vbInliers, float &score, cv::Mat &F21);
// 被FindHomography函数调用具体来算Homography矩阵
cv::Mat ComputeH21(const vector &vP1, const vector &vP2);
// 被FindFundamental函数调用具体来算Fundamental矩阵
cv::Mat ComputeF21(const vector &vP1, const vector &vP2);
// 被FindHomography函数调用,具体来算假设使用Homography模型的得分
float CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector &vbMatchesInliers, float sigma);
// 被FindFundamental函数调用,具体来算假设使用Fundamental模型的得分
float CheckFundamental(const cv::Mat &F21, vector &vbMatchesInliers, float sigma);
// 分解F矩阵,并从分解后的多个解中找出合适的R,t
bool ReconstructF(vector &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,
cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated);
// 分解H矩阵,并从分解后的多个解中找出合适的R,t
bool ReconstructH(vector &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated);
// 通过三角化方法,利用反投影矩阵将特征点恢复为3D点
void Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D);
// 归一化三维空间点和帧间位移t
void Normalize(const vector &vKeys, vector &vNormalizedPoints, cv::Mat &T);
// ReconstructF调用该函数进行cheirality check,从而进一步找出F分解后最合适的解
int CheckRT(const cv::Mat &R, const cv::Mat &t, const vector &vKeys1, const vector &vKeys2,
const vector &vMatches12, vector &vbInliers,
const cv::Mat &K, vector &vP3D, float th2, vector &vbGood, float ¶llax);
// F矩阵通过结合内参可以得到Essential矩阵,该函数用于分解E矩阵,将得到4组解
void DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t);
// Keypoints from Reference Frame (Frame 1)
vector mvKeys1; ///< 存储Reference Frame中的特征点
// Keypoints from Current Frame (Frame 2)
vector mvKeys2; ///< 存储Current Frame中的特征点
// Current Matches from Reference to Current
// Reference Frame: 1, Current Frame: 2
vector mvMatches12; ///< Match的数据结构是pair,mvMatches12只记录Reference到Current匹配上的特征点对
vector mvbMatched1; ///< 记录Reference Frame的每个特征点在Current Frame是否有匹配的特征点
// Calibration
cv::Mat mK; ///< 相机内参
// Standard Deviation and Variance
float mSigma, mSigma2; ///< 测量误差
// Ransac max iterations
int mMaxIterations; ///< 算Fundamental和Homography矩阵时RANSAC迭代次数
// Ransac sets
vector > mvSets; ///< 二维容器,外层容器的大小为迭代次数,内层容器大小为每次迭代算H或F矩阵需要的点
};
使用构造函数Initializer指定参考帧构造初始化器
函数Initialize,指定当前帧,恢复出两帧之间的相对姿态以及点云。
Initialize函数:
/**
* @brief 并行地计算基础矩阵和单应性矩阵,选取其中一个模型,恢复出最开始两帧之间的相对姿态以及点云
*/
bool Initializer::Initialize(const Frame &CurrentFrame, const vector &vMatches12, cv::Mat &R21, cv::Mat &t21,
vector &vP3D, vector &vbTriangulated)
{
// Fill structures with current keypoints and matches with reference frame
// Reference Frame: 1, Current Frame: 2
// Frame2 特征点
mvKeys2 = CurrentFrame.mvKeysUn;
// mvMatches12记录匹配上的特征点对
mvMatches12.clear();
mvMatches12.reserve(mvKeys2.size());
// mvbMatched1记录每个特征点是否有匹配的特征点,
// 这个变量后面没有用到,后面只关心匹配上的特征点
mvbMatched1.resize(mvKeys1.size());
// 步骤1:组织特征点对
for(size_t i=0, iend=vMatches12.size();i=0)
{
mvMatches12.push_back(make_pair(i,vMatches12[i]));
mvbMatched1[i]=true;
}
else
mvbMatched1[i]=false;
}
// 匹配上的特征点的个数
const int N = mvMatches12.size();
// Indices for minimum set selection
// 新建一个容器vAllIndices,生成0到N-1的数作为特征点的索引
vector vAllIndices;
vAllIndices.reserve(N);
vector vAvailableIndices;
for(int i=0; i >(mMaxIterations,vector(8,0));
DUtils::Random::SeedRandOnce(0);
for(int it=0; it vbMatchesInliersH, vbMatchesInliersF;
float SH, SF; // score for H and F
cv::Mat H, F; // H and F
// ref是引用的功能:http://en.cppreference.com/w/cpp/utility/functional/ref
// 计算homograpy并打分
thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
// 计算fundamental matrix并打分
thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
// Wait until both threads have finished
threadH.join();
threadF.join();
// Compute ratio of scores
// 步骤4:计算得分比例,选取某个模型
float RH = SH/(SH+SF);
// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
// 步骤5:从H矩阵或F矩阵中恢复R,t
if(RH>0.40)
return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
else //if(pF_HF>0.6)
return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
return false;
}
注意的要点: