ORB-SLAM2
代码
//在完成评分后,开始使用单应矩阵或者基础阵进行位姿的恢复
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);
首先,假设RH>0.4,使用单应阵来恢复位姿
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)
//传参:匹配的内点、参考帧到当前帧的H阵,相机的内参阵、计算得出的相机旋转阵、计算出的平移、世界坐标系下三角化测量特征点对之后得到的特征点的空间坐标、特征点被三角化的标记、三角化测量时需要马努的最小视角、恢复运动需要最少三角化测量成功的点的个数
{
int N=0;//统计内点匹配的个数
for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)//遍历内点匹配的size
if(vbMatchesInliers[i])//查看该索引对应的匹配对是否为内点
N++;
cv::Mat invK = K.inv();
cv::Mat A = invK*H21*K;//K-1*H21*KVV
cv::Mat U,w,Vt,V;
cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);//对A进行奇异值分解
V=Vt.t();
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);
if(d1/d2<1.00001 || d2/d3<1.00001)//奇异值的排布一般是从大到小的
{
return false;//如果不是,则返回
}
vector<cv::Mat> vR, vt, vn;
vR.reserve(8);//定义8中情况的R,t
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 d>0与d<0的两种情况
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);
}
//对8组解进行验证,选择在相机前方最多的3D最优解
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
for(size_t i=0; i<8; i++)
{
float parallaxi;//第i组解对应较大的视交差
vector<cv::Point3f> vP3Di//三角化后空间点的坐标
vector<bool> vbTriangulatedi;//特征点是否被三角化
int nGood = CheckRT(vR[i],vt[i],mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K,vP3Di, 4.0*mSigma2, vbTriangulatedi, parallaxi);//计算good的数目点,调用Initializer::CheckRT
if(nGood>bestGood)
{
secondBestGood = bestGood;
bestGood = nGood;
bestSolutionIdx = i;
bestParallax = parallaxi;
bestP3D = vP3Di;
bestTriangulated = vbTriangulatedi;
}
else if(nGood>secondBestGood)
{
secondBestGood = nGood;
}
}
进入CheckRT()
//利用R t来进行三角化,并看R t是否正确
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);//参考帧中的特征点是否是good
vP3D.resize(vKeys1.size());//将三维点的大小重设
vector<float> vCosParallax;//每对特诊点的视差
vCosParallax.reserve(vKeys1.size());
// Camera 1 Projection Matrix K[I|0]
cv::Mat P1(3,4,CV_32F,cv::Scalar(0));//计算相机的投影阵第一帧
K.copyTo(P1.rowRange(0,3).colRange(0,3));
cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);//第一个相机的光心
// Camera 2 Projection Matrix K[R|t]
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;
cv::Mat O2 = -R.t()*t;//第二个相机光心在世界坐标系下的位置
int nGood=0;
for(size_t i=0, iend=vMatches12.size();i<iend;i++)//遍历
{
if(!vbMatchesInliers[i])//如果不是内点的匹配点,跳过
continue;
const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];//是的化取出1,2中的keyponints
const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];
cv::Mat p3dC1;
Triangulate(kp1,kp2,P1,P2,p3dC1);//三角化计算三维点
if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2)))//检查它里面的一些值是不是无限值即它合不合法
{
vbGood[vMatches12[i].first]=false;//记为false
continue;
}
// Check parallax
cv::Mat normal1 = p3dC1 - O1;//通过三维点的正负,以及两光心视差大小来检验是否合法
float dist1 = cv::norm(normal1);//三维点到光心1的模长
cv::Mat normal2 = p3dC1 - O2;
float dist2 = cv::norm(normal2);//光心2到三维点的模长
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)
if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)//z是否为负的,夹角<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;//转到相机2坐标系下
if(p3dC2.at<float>(2)<=0 && cosParallax<0.99998)//计算z值与夹角
continue;
// Check reprojection error in first image
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;
vCosParallax.push_back(cosParallax);//统计检验的3点个数
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;
}
if(nGood>0)
{
sort(vCosParallax.begin(),vCosParallax.end());//按照余弦值进行排序
size_t idx = min(50,int(vCosParallax.size()-1));
parallax = acos(vCosParallax[idx])*180/CV_PI;//转换为弧度制
}
else
parallax=0;
return nGood;
}