1、 Initialize函数
mvIniMatches:帧1特征点在帧2中的匹配
//Rcw,tcw:待求相机位姿
//mvIniP3D:成功匹配点的空间3D坐标
//vbTriangulated:成功三角测量--ture,三角测量失败--false
bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,
vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
{
// Fill structures with current keypoints and matches with reference frame
// Reference Frame: 1, Current Frame: 2
mvKeys2 = CurrentFrame.mvKeysUn;
//清除内存 分配空间 重新定义尺寸
mvMatches12.clear();
mvMatches12.reserve(mvKeys2.size());
mvbMatched1.resize(mvKeys1.size());
//把帧1与帧2的对应索引放入mvMatches12,true标志成功, false标志失败
for(size_t i=0, iend=vMatches12.size();i<iend; i++)
{
if(vMatches12[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
vector<size_t> vAllIndices;
vAllIndices.reserve(N);
vector<size_t> vAvailableIndices;
for(int i=0; i<N; i++)
{
vAllIndices.push_back(i);
}
// Generate sets of 8 points for each RANSAC iteration
//每一个RANSAC iteration都生成八对点 总共mMaxIterations组
mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));
//按照一定的规则产生随即种子
DUtils::Random::SeedRandOnce(0);
//遍历所有组对
for(int it=0; it<mMaxIterations; it++)
{
//索引赋给中间变量
vAvailableIndices = vAllIndices;
// Select a minimum set
//随机选取 mMaxIterations 组,每组包含8个点,存入 mvSets 中
for(size_t j=0; j<8; j++)
{
//随即组合八个点
int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
//提取索引号
int idx = vAvailableIndices[randi];
//索引号赋给mvsets
mvSets[it][j] = idx;
//删除中间变量的索引
vAvailableIndices[randi] = vAvailableIndices.back();
vAvailableIndices.pop_back();
}
}
// Launch threads to compute in parallel a fundamental matrix and a homography
//多线程并行计算本质矩阵和单应矩阵
vector<bool> vbMatchesInliersH, vbMatchesInliersF;
float SH, SF;
cv::Mat H, F;
//std::thread m_thrProcess(&类名::函数名, &类对象, 函数参数);计算F H 得分
//thread的方法传递引用的时候,必须外层用ref来进行引用传递,否则就是浅拷贝(b=a a的地址赋值给b,a变了b也变)。
thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
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
//计算得分来选择用F还是H计算
float RH = SH/(SH+SF);
// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
//RH>0.4用H计算,否则用F计算
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;
}
2、FindHomography函数详解
这边计算一下本质矩阵,并计算出T:
FindHomography函数详解
3、ReconstructH函数
从本质矩阵中恢复出R t等参数