Initializer是用来初始化的，初始化的方法是根据当前帧和参考帧匹配得到的特征点对，利用RANSAC方法去计算单应性矩阵H和基础矩阵F，然后根据重投影误差大小决定选择哪个矩阵，最后使用SFM方法利用矩阵计算旋转R和平移T。
这里之所以要同时计算单应性矩阵H和基础矩阵F，是因为当特征点在同一个平面上时，基础矩阵F会出现退化现象，导致位姿计算错误。这也导致这个类的代码中，所有关于恢复矩阵的操作都有两个函数与之对应。
关于初始化所使用的理论内容，高翔《视觉SLAM十四讲》第7章有详细的解释，本篇文章更侧重代码的解释。
初始化中的主要函数列在下图中，以直观解释该类的工作流程（这张图是用一个试用版软件画的，到处图片时有水印，懒得去水印了，大家能看清就行）
下面就详细解释各个函数的意义
1. Initializer：设置参考帧和参数
Initializer(
const Frame &ReferenceFrame,//参考帧
float sigma, //计算矩阵得分时候所用的参数
int iterations)//RANSAC迭代次数
该函数的作用是设置参考帧，并设置初始化方法的参数。功能比较简单，不列出详细代码了。
2. Initialize：执行初始化
Initialize(
const Frame &CurrentFrame, //当前帧
const vector<int> &vMatches12, //ORB计算的初步匹配结果
cv::Mat &R21,//输出的旋转矩阵
cv::Mat &t21,//输出的平移向量
vector<cv::Point3f> &vP3D, //三角化重投影成功的匹配点的3d点在相机1下的坐标
vector<bool> &vbTriangulated)//初始化成功后，特征点中三角化投影是否成功的标志位
这个函数包含了整个初始化的全部流程，主要包括以下步骤：
1）重新组织特征点对。其实就是重新弄了一下数据结构，把匹配的点对序号放在一起，方便后面使用
2）特征点对分组。这一步主要是为了给RANSAC使用，对特征点对按照RANSAC循环次数随机分组。
3）两个线程同时计算单应性矩阵H和本质矩阵F
4）根据三角化成功点数来判断是选单应性矩阵H和本质矩阵F中的哪一个
5）根据矩阵，使用SFM方法恢复R和T
详细代码注释如下
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
// Frame2 特征点
mvKeys2 = CurrentFrame.mvKeysUn;
//mvMatches12储存着匹配点对在参考帧F1和当前帧F2中的序号
mvMatches12.clear();
mvMatches12.reserve(mvKeys2.size());
// mvbMatched1记录每个特征点是否有匹配的特征点
mvbMatched1.resize(mvKeys1.size());
// 步骤1：组织特征点对
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
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
// 步骤2：在所有匹配特征点对中随机选择8对匹配特征点为一组，共选择mMaxIterations组
// 用于FindHomography和FindFundamental求解
// mMaxIterations:200
mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));
DUtils::Random::SeedRandOnce(0);
//RANSAC循环mMaxIterations次
for(int it=0; it<mMaxIterations; it++)
{
vAvailableIndices = vAllIndices;
// Select a minimum set
for(size_t j=0; j<8; j++)
{
int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
int idx = vAvailableIndices[randi];
mvSets[it][j] = idx;
vAvailableIndices[randi] = vAvailableIndices.back();
vAvailableIndices.pop_back();
}
}
// Launch threads to compute in parallel a fundamental matrix and a homography
// 步骤3：调用多线程分别用于计算fundamental matrix和homography
vector<bool> vbMatchesInliersH, vbMatchesInliersF;
//SH计算单应矩阵的得分，SF计算基础矩阵得分
float SH, SF;
cv::Mat H, F;
// 计算homograpy和得分
thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
// 计算fundamental和得分
thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
// Wait until both threads have finished
//在这里等待线程threadH，threadF结束才往下继续执行
//也就是等待SH，SF的结果
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;
}
3. FindHomography：计算单应矩阵及其得分
FindHomography(
vector<bool> &vbMatchesInliers, //匹配点中哪些可以通过H21重投影成功
float &score, //得分
cv::Mat &H21)//输出的单应性矩阵
该函数的主要流程如下：
1）特征点归一化
2）计算单应性矩阵ComputeH21
3）计算2中矩阵对应的得分
4）按照设定的RANSAC循环次数循环执行2）和3），并找到得分最高的那个矩阵
代码还算简单，容易理解
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
{
// Number of putative matches
// 假定匹配的数量
const int N = mvMatches12.size();
// Normalize coordinates
vector<cv::Point2f> vPn1, vPn2;
cv::Mat T1, T2;
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
cv::Mat T2inv = T2.inv();
// Best Results variables
score = 0.0;
vbMatchesInliers = vector<bool>(N,false);
// Iteration variables
vector<cv::Point2f> vPn1i(8);
vector<cv::Point2f> vPn2i(8);
cv::Mat H21i, H12i;
vector<bool> vbCurrentInliers(N,false);
float currentScore;
// Perform all RANSAC iterations and save the solution with highest score
// 在所有RANSAC样本中寻找能够使重投影的点对数达到最多的样本
for(int it=0; it<mMaxIterations; it++)
{
// Select a minimum set
for(size_t j=0; j<8; j++)
{
int idx = mvSets[it][j];
vPn1i[j] = vPn1[mvMatches12[idx].first];
vPn2i[j] = vPn2[mvMatches12[idx].second];
}
//计算本次RANSAC样本下的单应矩阵
cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
H21i = T2inv*Hn*T1;
H12i = H21i.inv();
//在参数 mSigma下，能够通过H21，H12重投影成功的点有哪些，并返回分数
currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
if(currentScore>score)
{
H21 = H21i.clone();
vbMatchesInliers = vbCurrentInliers;
score = currentScore;
}
}
}
4. FindFundamental：计算基础矩阵及其得分
FindFundamental(
vector<bool> &vbMatchesInliers, //匹配点中哪些可以通过H21重投影成功
float &score, //得分
cv::Mat &F21)//输出的本质矩阵
该函数的主要流程如下：
1）特征点归一化
2）计算基础矩阵ComputeF21
3）计算2中矩阵对应的得分
4）按照设定的RANSAC循环次数循环执行2）和3），并找到得分最高的那个矩阵
到这里其实已经发现它的代码和3中的FindHomography的代码非常像了，更容易理解了
void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
{
// Number of putative matches
const int N = vbMatchesInliers.size();
// Normalize coordinates
vector<cv::Point2f> vPn1, vPn2;
cv::Mat T1, T2;
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
cv::Mat T2t = T2.t();
// Best Results variables
score = 0.0;
vbMatchesInliers = vector<bool>(N,false);
// Iteration variables
vector<cv::Point2f> vPn1i(8);
vector<cv::Point2f> vPn2i(8);
cv::Mat F21i;
vector<bool> vbCurrentInliers(N,false);
float currentScore;
// Perform all RANSAC iterations and save the solution with highest score
// 在所有RANSAC样本中寻找能够使重投影的点对数达到最多的样本
for(int it=0; it<mMaxIterations; it++)
{
// Select a minimum set
for(int j=0; j<8; j++)
{
int idx = mvSets[it][j];
vPn1i[j] = vPn1[mvMatches12[idx].first];
vPn2i[j] = vPn2[mvMatches12[idx].second];
}
//计算出归一化特征点对应的基础矩阵
cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
//转换成归一化前特征点对应的基础矩阵
F21i = T2t*Fn*T1;
//在参数 mSigma下，能够通过F21li，
//重投影成功的点有哪些，并返回分数
currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);
if(currentScore>score)
{
F21 = F21i.clone();
vbMatchesInliers = vbCurrentInliers;
score = currentScore;
}
}
}
5. ComputeH21：计算单应性矩阵
ComputeH21(
const vector<cv::Point2f> &vP1, //帧1中的特征点
const vector<cv::Point2f> &vP2)//帧2中的特征点
其作用就是计算单应性矩阵，代码不在这里列出了，就是翻译公式而已，没别的
6. ComputeF21：计算基础矩阵
ComputeF21(
const vector<cv::Point2f> &vP1, //帧1中的特征点
const vector<cv::Point2f> &vP2)//帧2中的特征点
也是翻译公式，不解释
7. CheckHomography：评估单应性矩阵
CheckHomography(
const cv::Mat &H21, //单应性矩阵
const cv::Mat &H12, //单应性矩阵的逆
vector<bool> &vbMatchesInliers, //匹配点重投影是否成功的标志位
float sigma)//计算得分时需要的参数
float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma)
{
const int N = mvMatches12.size();
const float h11 = H21.at<float>(0,0);
const float h12 = H21.at<float>(0,1);
const float h13 = H21.at<float>(0,2);
const float h21 = H21.at<float>(1,0);
const float h22 = H21.at<float>(1,1);
const float h23 = H21.at<float>(1,2);
const float h31 = H21.at<float>(2,0);
const float h32 = H21.at<float>(2,1);
const float h33 = H21.at<float>(2,2);
const float h11inv = H12.at<float>(0,0);
const float h12inv = H12.at<float>(0,1);
const float h13inv = H12.at<float>(0,2);
const float h21inv = H12.at<float>(1,0);
const float h22inv = H12.at<float>(1,1);
const float h23inv = H12.at<float>(1,2);
const float h31inv = H12.at<float>(2,0);
const float h32inv = H12.at<float>(2,1);
const float h33inv = H12.at<float>(2,2);
vbMatchesInliers.resize(N);
float score = 0;
//判断通过单应矩阵重投影是否成功的阈值
const float th = 5.991;
const float invSigmaSquare = 1.0/(sigma*sigma);
//遍历所有N对特征匹配点
for(int i=0; i<N; i++)
{
bool bIn = true;
const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
const float u1 = kp1.pt.x;
const float v1 = kp1.pt.y;
const float u2 = kp2.pt.x;
const float v2 = kp2.pt.y;
// Reprojection error in first image
// x2in1 = H12*x2
// 将图像2中的特征点单应到图像1中
const float w2in1inv = 1.0/(h31inv*u2+h32inv*v2+h33inv);
const float u2in1 = (h11inv*u2+h12inv*v2+h13inv)*w2in1inv;
const float v2in1 = (h21inv*u2+h22inv*v2+h23inv)*w2in1inv;
// 计算u2，v2投影到F1后与u1,v1的距离的平方，也就是重投影误差
const float squareDist1 = (u1-u2in1)*(u1-u2in1)+(v1-v2in1)*(v1-v2in1);
// 根据方差归一化误差
const float chiSquare1 = squareDist1*invSigmaSquare;
//chiSquare1>th说明匹配的点对F1投影到F2，重投影失败
if(chiSquare1>th)
bIn = false;
else
score += th - chiSquare1;
// Reprojection error in second image
// x1in2 = H21*x1
// 将图像1中的特征点单应到图像2中
const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);
const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;
const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;
const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);
const float chiSquare2 = squareDist2*invSigmaSquare;
if(chiSquare2>th)
bIn = false;
else
score += th - chiSquare2;
//bIn标志着此对匹配点是否重投影成功
if(bIn)
vbMatchesInliers[i]=true;
else
vbMatchesInliers[i]=false;
}
return score;
}
8. CheckFundamental：评估基础矩阵
CheckFundamental(
const cv::Mat &F21, //基础矩阵
vector<bool> &vbMatchesInliers, //匹配点重投影是否成功的标志位
float sigma)//计算得分时需要的参数
float Initializer::CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma)
{
const int N = mvMatches12.size();
const float f11 = F21.at<float>(0,0);
const float f12 = F21.at<float>(0,1);
const float f13 = F21.at<float>(0,2);
const float f21 = F21.at<float>(1,0);
const float f22 = F21.at<float>(1,1);
const float f23 = F21.at<float>(1,2);
const float f31 = F21.at<float>(2,0);
const float f32 = F21.at<float>(2,1);
const float f33 = F21.at<float>(2,2);
vbMatchesInliers.resize(N);
float score = 0;
const float th = 3.841;
const float thScore = 5.991;
const float invSigmaSquare = 1.0/(sigma*sigma);
for(int i=0; i<N; i++)
{
bool bIn = true;
const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
const float u1 = kp1.pt.x;
const float v1 = kp1.pt.y;
const float u2 = kp2.pt.x;
const float v2 = kp2.pt.y;
// Reprojection error in second image
// l2=F21x1=(a2,b2,c2)
// F21*x1可以算出x1在图像中x2对应的线l
const float a2 = f11*u1+f12*v1+f13;
const float b2 = f21*u1+f22*v1+f23;
const float c2 = f31*u1+f32*v1+f33;
// x2应该在l这条线上:x2点乘l = 0
const float num2 = a2*u2+b2*v2+c2;
const float squareDist1 = num2*num2/(a2*a2+b2*b2);
const float chiSquare1 = squareDist1*invSigmaSquare;
if(chiSquare1>th)
bIn = false;
else
score += thScore - chiSquare1;
// Reprojection error in second image
// l1 =x2tF21=(a1,b1,c1)
const float a1 = f11*u2+f21*v2+f31;
const float b1 = f12*u2+f22*v2+f32;
const float c1 = f13*u2+f23*v2+f33;
const float num1 = a1*u1+b1*v1+c1;
const float squareDist2 = num1*num1/(a1*a1+b1*b1);
const float chiSquare2 = squareDist2*invSigmaSquare;
if(chiSquare2>th)
bIn = false;
else
score += thScore - chiSquare2;
if(bIn)
vbMatchesInliers[i]=true;
else
vbMatchesInliers[i]=false;
}
return score;
}
9. ReconstructH：从单应性矩阵恢复位姿
ReconstructH(
vector<bool> &vbMatchesInliers, //匹配点中哪些可以通过H21重投影成功
cv::Mat &H21, //单应性矩阵
cv::Mat &K,//内参
cv::Mat &R21, //旋转矩阵
cv::Mat &t21, //平移向量
vector<cv::Point3f> &vP3D, //三角化重投影成功的匹配点的3d点在相机1下的坐标
vector<bool> &vbTriangulated, //特征点是否重投影成功的标志位
float minParallax, //设置的最小视差角余弦值参数，输出Rt模型的视差角小于此值则返回失败
int minTriangulated)//匹配点中H21重投影成功的个数如果小于此值，返回失败
其实就是SFM方法，由于恢复的位姿不唯一，需要根据恢复的特征点位姿来判断哪个是正确的。
bool Initializer::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)
{
//N，通过H重投影成功的数量
int N=0;
for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
if(vbMatchesInliers[i])
N++;
// We recover 8 motion hypotheses using the method of Faugeras et al.
// Motion and structure from motion in a piecewise planar environment.
// International Journal of Pattern Recognition and Artificial Intelligence, 1988
// 将H矩阵由图像坐标系变换到相机坐标系
cv::Mat invK = K.inv();
cv::Mat A = invK*H21*K;
cv::Mat U,w,Vt,V;
cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);
//vt转置
V=Vt.t();
//cv::determinant(U)为U的行列式
float s = cv::determinant(U)*cv::determinant(Vt);
float d1 = w.at<float>(0);
float d2 = w.at<float>(1);
float d3 = w.at<float>(2);
//注意d1>d2>d3
//看吴博讲解的ppt19页，只考虑d1!=d2!=d3的情况，其他情况返回失败
if(d1/d2<1.00001 || d2/d3<1.00001)
{
return false;
}
vector<cv::Mat> vR, vt, vn;
vR.reserve(8);
vt.reserve(8);
vn.reserve(8);
//n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1
float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));
float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));
float x1[] = {aux1,aux1,-aux1,-aux1};
float x3[] = {aux3,-aux3,aux3,-aux3};
//case d'=d2
float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);
float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);
float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};
for(int i=0; i<4; i++)
{
cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
Rp.at<float>(0,0)=ctheta;
Rp.at<float>(0,2)=-stheta[i];
Rp.at<float>(2,0)=stheta[i];
Rp.at<float>(2,2)=ctheta;
cv::Mat R = s*U*Rp*Vt;
vR.push_back(R);
cv::Mat tp(3,1,CV_32F);
tp.at<float>(0)=x1[i];
tp.at<float>(1)=0;
tp.at<float>(2)=-x3[i];
tp*=d1-d3;
cv::Mat t = U*tp;
vt.push_back(t/cv::norm(t));
cv::Mat np(3,1,CV_32F);
np.at<float>(0)=x1[i];
np.at<float>(1)=0;
np.at<float>(2)=x3[i];
cv::Mat n = V*np;
if(n.at<float>(2)<0)
n=-n;
vn.push_back(n);
}
//case d'=-d2
float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);
float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);
float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};
for(int i=0; i<4; i++)
{
cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
Rp.at<float>(0,0)=cphi;
Rp.at<float>(0,2)=sphi[i];
Rp.at<float>(1,1)=-1;
Rp.at<float>(2,0)=sphi[i];
Rp.at<float>(2,2)=-cphi;
cv::Mat R = s*U*Rp*Vt;
vR.push_back(R);
cv::Mat tp(3,1,CV_32F);
tp.at<float>(0)=x1[i];
tp.at<float>(1)=0;
tp.at<float>(2)=x3[i];
tp*=d1+d3;
cv::Mat t = U*tp;
vt.push_back(t/cv::norm(t));
cv::Mat np(3,1,CV_32F);
np.at<float>(0)=x1[i];
np.at<float>(1)=0;
np.at<float>(2)=x3[i];
cv::Mat n = V*np;
if(n.at<float>(2)<0)
n=-n;
vn.push_back(n);
}
int bestGood = 0;
int secondBestGood = 0;
int bestSolutionIdx = -1;
float bestParallax = -1;
vector<cv::Point3f> bestP3D;
vector<bool> bestTriangulated;
// Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax)
// We reconstruct all hypotheses and check in terms of triangulated points and parallax
//经过上面的计算，共有8种R、t计算结果，遍历这8种可能模型
//通过计算出匹配点的三角化重投影成功的数量，来找出最好模型和次好模型
for(size_t i=0; i<8; i++)
{
float parallaxi;
vector<cv::Point3f> vP3Di;
vector<bool> vbTriangulatedi;
//计算在输入Rt下，匹配点三角化重投影成功的数量
int nGood = CheckRT(vR[i],vt[i],mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K,vP3Di, 4.0*mSigma2, vbTriangulatedi, parallaxi);
if(nGood>bestGood)
{
secondBestGood = bestGood;
bestGood = nGood;
bestSolutionIdx = i;
bestParallax = parallaxi;
bestP3D = vP3Di;
bestTriangulated = vbTriangulatedi;
}
else if(nGood>secondBestGood)
{
secondBestGood = nGood;
}
}
//secondBestGood<0.75*bestGood 如果最好模型与次好模型差距足够大
//bestParallax>=minParallax 最好模型对应的视差角大于此值
//bestGood>minTriangulated 最好模型对应的匹配点三角化重投影成功数量大于此阈值
//bestGood>0.9*N 匹配点三角化重投影成功数量占通过H重投影成功数量的比例需要大于0.9
if(secondBestGood<0.75*bestGood && bestParallax>=minParallax && bestGood>minTriangulated && bestGood>0.9*N)
{
vR[bestSolutionIdx].copyTo(R21);
vt[bestSolutionIdx].copyTo(t21);
vP3D = bestP3D;
vbTriangulated = bestTriangulated;
return true;
}
return false;
}
10. ReconstructF：从单应性矩阵恢复位姿
ReconstructF(
vector<bool> &vbMatchesInliers, //匹配点中哪些可以通过H21重投影成功
cv::Mat &F21, //基础矩阵
cv::Mat &K,//内参
cv::Mat &R21, //旋转矩阵
cv::Mat &t21, //平移向量
vector<cv::Point3f> &vP3D, //三角化重投影成功的匹配点的3d点在相机1下的坐标
vector<bool> &vbTriangulated, //特征点是否重投影成功的标志位
float minParallax, //设置的最小视差角余弦值参数，输出Rt模型的视差角小于此值则返回失败
int minTriangulated)//匹配点中H21重投影成功的个数如果小于此值，返回失败
没什么好解释的了
bool Initializer::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)
{
int N=0;
for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
if(vbMatchesInliers[i])
N++;
// Compute Essential Matrix from Fundamental Matrix
// 基本矩阵结合相机内参得到本质矩阵
cv::Mat E21 = K.t()*F21*K;
cv::Mat R1, R2, t;
// Recover the 4 motion hypotheses
// 分解本质矩阵
DecomposeE(E21,R1,R2,t);
cv::Mat t1=t;
cv::Mat t2=-t;
// Reconstruct with the 4 hyphoteses and check
// 检查四种解
vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;
vector<bool> vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4;
float parallax1,parallax2, parallax3, parallax4;
int nGood1 = CheckRT(R1,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D1, 4.0*mSigma2, vbTriangulated1, parallax1);
int nGood2 = CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2);
int nGood3 = CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3);
int nGood4 = CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4);
int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4)));
R21 = cv::Mat();
t21 = cv::Mat();
// minTriangulated为可以三角化恢复三维点的个数
int nMinGood = max(static_cast<int>(0.9*N),minTriangulated);
int nsimilar = 0;
if(nGood1>0.7*maxGood)
nsimilar++;
if(nGood2>0.7*maxGood)
nsimilar++;
if(nGood3>0.7*maxGood)
nsimilar++;
if(nGood4>0.7*maxGood)
nsimilar++;
// If there is not a clear winner or not enough triangulated points reject initialization
//nsimilar>1表明没有哪个模型明显胜出
//匹配点三角化重投影成功数过少
// 四个结果中如果没有明显的最优结果，则返回失败
if(maxGood<nMinGood || nsimilar>1)
{
return false;
}
// If best reconstruction has enough parallax initialize
if(maxGood==nGood1)
{//如果模型一对应的视差角大于最小值
if(parallax1>minParallax)
{
vP3D = vP3D1;
vbTriangulated = vbTriangulated1;
R1.copyTo(R21);
t1.copyTo(t21);
return true;
}
}else if(maxGood==nGood2)
{
if(parallax2>minParallax)
{
vP3D = vP3D2;
vbTriangulated = vbTriangulated2;
R2.copyTo(R21);
t1.copyTo(t21);
return true;
}
}else if(maxGood==nGood3)
{
if(parallax3>minParallax)
{
vP3D = vP3D3;
vbTriangulated = vbTriangulated3;
R1.copyTo(R21);
t2.copyTo(t21);
return true;
}
}else if(maxGood==nGood4)
{
if(parallax4>minParallax)
{
vP3D = vP3D4;
vbTriangulated = vbTriangulated4;
R2.copyTo(R21);
t2.copyTo(t21);
return true;
}
}
return false;
}
11. Normalize：特征点归一化
Normalize(
const vector<cv::KeyPoint> &vKeys, //待归一化特征点集合
vector<cv::Point2f> &vNormalizedPoints, //归一化后特征点集合
cv::Mat &T)//归一化所使用的矩阵
将一个特征点集合归一化到另一个坐标系，使得归一化后的坐标点集合均值为0，一阶绝对矩为1，这样计算矩阵更准确
void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T)
{
float meanX = 0;
float meanY = 0;
const int N = vKeys.size();
vNormalizedPoints.resize(N);
for(int i=0; i<N; i++)
{
meanX += vKeys[i].pt.x;
meanY += vKeys[i].pt.y;
}
meanX = meanX/N;
meanY = meanY/N;
float meanDevX = 0;
float meanDevY = 0;
// 将所有vKeys点减去中心坐标，使x坐标和y坐标均值分别为0
for(int i=0; i<N; i++)
{
vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;
vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;
meanDevX += fabs(vNormalizedPoints[i].x);
meanDevY += fabs(vNormalizedPoints[i].y);
}
meanDevX = meanDevX/N;
meanDevY = meanDevY/N;
float sX = 1.0/meanDevX;
float sY = 1.0/meanDevY;
// 将x坐标和y坐标分别进行缩放，使得x坐标和y坐标的一阶绝对矩分别为1
for(int i=0; i<N; i++)
{
vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;
vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;
}
T = cv::Mat::eye(3,3,CV_32F);
T.at<float>(0,0) = sX;
T.at<float>(1,1) = sY;
T.at<float>(0,2) = -meanX*sX;
T.at<float>(1,2) = -meanY*sY;
}
12. CheckRT：计算三角化重投影成功的数量
CheckRT(
const cv::Mat &R, //旋转矩阵
const cv::Mat &t, //平移矩阵
const vector<cv::KeyPoint> &vKeys1, //帧1的特征点
const vector<cv::KeyPoint> &vKeys2, //帧2的特征点
const vector<Match> &vMatches12, //orbmatcher计算的初匹配
vector<bool> &vbMatchesInliers,//匹配点中哪些可以通过H或者F重投影成功
const cv::Mat &K, //相机内参
vector<cv::Point3f> &vP3D, //三角化重投影成功的匹配点的3d点在相机1下的坐标
float th2, //根据三角化重投影误差判断匹配点是否重投影成功的阈值
vector<bool> &vbGood, //特征点哪些三角化重投影成功
float ¶llax)//三角化重投影成功匹配点的视差角
直接上代码
int Initializer::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> &vbMatchesInliers,
const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float ¶llax)
{
// Calibration parameters
const float fx = K.at<float>(0,0);
const float fy = K.at<float>(1,1);
const float cx = K.at<float>(0,2);
const float cy = K.at<float>(1,2);
vbGood = vector<bool>(vKeys1.size(),false);
vP3D.resize(vKeys1.size());
vector<float> vCosParallax;
vCosParallax.reserve(vKeys1.size());
// Camera 1 Projection Matrix K[I|0]
//相机1的投影矩阵K[I|0]，世界坐标系和相机1坐标系相同
cv::Mat P1(3,4,CV_32F,cv::Scalar(0));
K.copyTo(P1.rowRange(0,3).colRange(0,3));
// 相机1的光心在世界坐标系坐标
cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);
// Camera 2 Projection Matrix K[R|t]
//相机2的投影矩阵
cv::Mat P2(3,4,CV_32F);
R.copyTo(P2.rowRange(0,3).colRange(0,3));
t.copyTo(P2.rowRange(0,3).col(3));
P2 = K*P2;
// 相机2的光心在世界坐标系坐标
cv::Mat O2 = -R.t()*t;
int nGood=0;
//遍历所有的匹配点
for(size_t i=0, iend=vMatches12.size();i<iend;i++)
{
//如果在
if(!vbMatchesInliers[i])
continue;
// kp1和kp2是匹配特征点
const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];
const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];
//3d点在相机1和世界坐标系下的坐标
cv::Mat p3dC1;
//输出的p3dC1是综合考虑了P1,P2的kp1,kp2匹配点在世界坐标系中的齐次坐标
//由于世界坐标系和相机1坐标系重合，所以p3dC1同时也是匹配点对应的空间点在相机1坐标系中的坐标
Triangulate(kp1,kp2,P1,P2,p3dC1);
//isfinite()判断一个浮点数是否是一个有限值
//相当于是确定p3dC1前三位数值正常
if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2)))
{
vbGood[vMatches12[i].first]=false;
continue;
}
// Check parallax
//normal1是相机1到3d点的向量
cv::Mat normal1 = p3dC1 - O1;
float dist1 = cv::norm(normal1);
//normal2是相机2到3d点的向量
cv::Mat normal2 = p3dC1 - O2;
float dist2 = cv::norm(normal2);
//cosParallax为视差角的余弦，也就是normal1与normal2的余弦
float cosParallax = normal1.dot(normal2)/(dist1*dist2);
// Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)
// 判断3D点是否在两个摄像头前方
//p3dC1.at<float>(2)<=0说明3d点在光心后面，深度为负
//p3dC1视差角较大，且深度为负则淘汰
if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)
continue;
// Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)
cv::Mat p3dC2 = R*p3dC1+t;
//p3dC2视差角较大，且深度为负则淘汰
if(p3dC2.at<float>(2)<=0 && cosParallax<0.99998)
continue;
// Check reprojection error in first image
// 计算3D点在第一个图像上的投影误差
float im1x, im1y;
float invZ1 = 1.0/p3dC1.at<float>(2);
im1x = fx*p3dC1.at<float>(0)*invZ1+cx;
im1y = fy*p3dC1.at<float>(1)*invZ1+cy;
float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);
if(squareError1>th2)
continue;
// Check reprojection error in second image
float im2x, im2y;
float invZ2 = 1.0/p3dC2.at<float>(2);
im2x = fx*p3dC2.at<float>(0)*invZ2+cx;
im2y = fy*p3dC2.at<float>(1)*invZ2+cy;
float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);
// 重投影误差太大，淘汰
if(squareError2>th2)
continue;
// 统计经过检验的3D点个数，记录3D点视差角
// 到这里说明这对匹配点三角化重投影成功了
vCosParallax.push_back(cosParallax);
vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));
nGood++;
//确认视差角最够大
if(cosParallax<0.99998)
vbGood[vMatches12[i].first]=true;
}
// 得到3D点中较大的视差角
if(nGood>0)
{
//将视差角余弦有小到大排序
sort(vCosParallax.begin(),vCosParallax.end());
//取出第50个，或者最后那个也就是最大那个
size_t idx = min(50,int(vCosParallax.size()-1));
//计算出视差角
parallax = acos(vCosParallax[idx])*180/CV_PI;
}
else
parallax=0;
return nGood;
}
评论（0）
您还未登录，请登录后发表或查看评论