ORB-SLAM2梳理——track线程（三）
上一篇博客主要讲了track线程的特征点提取，以及描述子的计算，其中重点是特征点的提取。对于特征点提取，ORB-SLAM首先将图像建立图像金字塔，对每一层图像提取FAST角点。为了保证提取角点的分布比较均匀，将图像分成了一定数量的块，在提取角点之后，将特征点分散到八叉树中，然后计算相应的描述子。本篇博客接着分析track线程的具体实现。
首先分析特征点匹配函数（初始化时）。
ORBmatcher matcher(0.9,true); ///初始定义一个特征匹配类的对象
//构造结果： mfNNratio=0.9 mbCheckOrientation=true
参数：初始帧，当前帧，mvbPrevMatched,为前一帧的特征点，存储了mInitialFrame中哪些点将进行接下来的匹配，mvIniMatches存储mInitialFrame,mCurrentFrame之间匹配的特征点,键值对是两帧匹配特征点的索引
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
orb在初始化时对于特征点匹配很严格，最相近的匹配结果必须小于次相近的匹配结果的90%，并且不允许第一帧中的两个特征点在第二帧中有同一个最相近的匹配点。
int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector &vbPrevMatched, vector &vnMatches12, int windowSize)
{
int nmatches=0;
vnMatches12 = vector(F1.mvKeysUn.size(),-1);
vector rotHist[HISTO_LENGTH];
for(int i=0;i vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);
vector vnMatches21(F2.mvKeysUn.size(),-1);
for(size_t i1=0, iend1=F1.mvKeysUn.size(); i10)
continue;
///找到当前帧中以x, y为中心,边长为2r的方形内且在[minLevel, maxLevel]的特征点，返回满足条件的特征点的序号，加速匹配
vector vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);
if(vIndices2.empty())
continue;
cv::Mat d1 = F1.mDescriptors.row(i1);
int bestDist = INT_MAX;
int bestDist2 = INT_MAX;
int bestIdx2 = -1;
for(vector::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++)
{
size_t i2 = *vit;
cv::Mat d2 = F2.mDescriptors.row(i2); /// 仅与满足上述区域的特征点进行匹配
int dist = DescriptorDistance(d1,d2); ////特征点匹配函数
if(vMatchedDistance[i2]<=dist)
continue;
if(dist=0)
{
vnMatches12[vnMatches21[bestIdx2]]=-1;
nmatches--;
}
vnMatches12[i1]=bestIdx2;
vnMatches21[bestIdx2]=i1;
vMatchedDistance[bestIdx2]=bestDist;
nmatches++;
if(mbCheckOrientation) ///特征点的方向检测
{
float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle;
if(rot<0.0)
rot+=360.0f;
int bin = round(rot*factor);
if(bin==HISTO_LENGTH)
bin=0;
assert(bin>=0 && bin=0)
{
vnMatches12[idx1]=-1;
nmatches--;
}
}
}
}
//Update prev matched
for(size_t i1=0, iend1=vnMatches12.size(); i1=0)
vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;
return nmatches;
}
特征点匹配函数，计算描述子之间的距离。
参数：第一帧的特征点的描述子，第二帧特征点的描述子
int ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b)
{
const int *pa = a.ptr();
const int *pb = b.ptr();
int dist=0;
for(int i=0; i<8; i++, pa++, pb++)
{
unsigned int v = *pa ^ *pb;
v = v - ((v >> 1) & 0x55555555);
v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
}
return dist;
}
在特征点匹配之后，接下来就是初始化操作。
if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
对于初始化操作，从两帧相匹配的特征点中随机选取8对，迭代200次，采用两个线程分别计算基础矩阵ComputeF21(vPn1i,vPn2i);和单应矩阵ComputeH21(vPn1i,vPn2i); ，并且计算相应的得分CheckFundamental(F21i, vbCurrentInliers, mSigma);，CheckHomography(H21i, H12i, vbCurrentInliers, mSigma); ，然后在确定利用哪种方法初始化。
参数：当前帧（第二帧），第二帧中与第一帧的特征点相匹配的特征点序号，初始旋转，初始平移，3D点，三角化
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
mvKeys2 = CurrentFrame.mvKeysUn;
mvMatches12.clear();
mvMatches12.reserve(mvKeys2.size());
mvbMatched1.resize(mvKeys1.size());
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
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;
cv::Mat H, F;
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
float RH = SH/(SH+SF);
// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
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;
}
根据得分确定那种初始化方法，初始化方法分别为：
ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
//利用单应矩阵进行初始化
//参数：匹配点对是否inlier，单应矩阵，内参矩阵，初始旋转，初始平移，3D点，最小视差，最小三角化
bool Initializer::ReconstructH(vector &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated)
{
int N=0;
for(size_t i=0, iend = vbMatchesInliers.size() ; i(0);
float d2 = w.at(1);
float d3 = w.at(2);
if(d1/d2<1.00001 || d2/d3<1.00001)
{
return false;
}
vector 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(0,0)=ctheta;
Rp.at(0,2)=-stheta[i];
Rp.at(2,0)=stheta[i];
Rp.at(2,2)=ctheta;
cv::Mat R = s*U*Rp*Vt;
vR.push_back(R);
cv::Mat tp(3,1,CV_32F);
tp.at(0)=x1[i];
tp.at(1)=0;
tp.at(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(0)=x1[i];
np.at(1)=0;
np.at(2)=x3[i];
cv::Mat n = V*np;
if(n.at(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(0,0)=cphi;
Rp.at(0,2)=sphi[i];
Rp.at(1,1)=-1;
Rp.at(2,0)=sphi[i];
Rp.at(2,2)=-cphi;
cv::Mat R = s*U*Rp*Vt;
vR.push_back(R);
cv::Mat tp(3,1,CV_32F);
tp.at(0)=x1[i];
tp.at(1)=0;
tp.at(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(0)=x1[i];
np.at(1)=0;
np.at(2)=x3[i];
cv::Mat n = V*np;
if(n.at(2)<0)
n=-n;
vn.push_back(n);
}
int bestGood = 0;
int secondBestGood = 0;
int bestSolutionIdx = -1;
float bestParallax = -1;
vector bestP3D;
vector 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;
vector vP3Di;
vector vbTriangulatedi;
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;
}
}
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;
}
///从基础矩阵初始化
bool Initializer::ReconstructF(vector &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,
cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated)
{
int N=0;
for(size_t i=0, iend = vbMatchesInliers.size() ; i vP3D1, vP3D2, vP3D3, vP3D4;
vector 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();
int nMinGood = max(static_cast(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
if(maxGood1)
{
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;
}
在初始化完成之后，根据初始化过程中估计的位姿，以及三角测量恢复出的特征点的深度创建单目初始地图，并且利用G2O基于最小化重投影误差进行优化。
CreateInitialMapMonocular();
void Tracking::CreateInitialMapMonocular()
{
// Create KeyFrames
KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);
KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
pKFini->ComputeBoW();
pKFcur->ComputeBoW();
// Insert KFs in the map
mpMap->AddKeyFrame(pKFini);
mpMap->AddKeyFrame(pKFcur);
// Create MapPoints and asscoiate to keyframes
for(size_t i=0; iAddMapPoint(pMP,i);
pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
pMP->AddObservation(pKFini,i);
pMP->AddObservation(pKFcur,mvIniMatches[i]);
pMP->ComputeDistinctiveDescriptors();
pMP->UpdateNormalAndDepth();
//Fill Current Frame structure
mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;
//Add to Map
mpMap->AddMapPoint(pMP);
}
// Update Connections
pKFini->UpdateConnections();
pKFcur->UpdateConnections();
// Bundle Adjustment
cout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl;
Optimizer::GlobalBundleAdjustemnt(mpMap,20);
// Set median depth to 1
float medianDepth = pKFini->ComputeSceneMedianDepth(2);
float invMedianDepth = 1.0f/medianDepth;
if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
{
cout << "Wrong initialization, reseting..." << endl;
Reset();
return;
}
// Scale initial baseline
cv::Mat Tc2w = pKFcur->GetPose();
Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
pKFcur->SetPose(Tc2w);
// Scale points
vector vpAllMapPoints = pKFini->GetMapPointMatches();
for(size_t iMP=0; iMPSetWorldPos(pMP->GetWorldPos()*invMedianDepth);
}
}
////将初始化用到的两帧作为关键帧，并设置第二帧为接下来第三帧跟踪的参考帧。
mpLocalMapper->InsertKeyFrame(pKFini);
mpLocalMapper->InsertKeyFrame(pKFcur);
mCurrentFrame.SetPose(pKFcur->GetPose());
mnLastKeyFrameId=mCurrentFrame.mnId;
mpLastKeyFrame = pKFcur;
mvpLocalKeyFrames.push_back(pKFcur);
mvpLocalKeyFrames.push_back(pKFini);
mvpLocalMapPoints=mpMap->GetAllMapPoints();
mpReferenceKF = pKFcur;
mCurrentFrame.mpReferenceKF = pKFcur;
mLastFrame = Frame(mCurrentFrame);
mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());
mpMap->mvpKeyFrameOrigins.push_back(pKFini);
mState=OK; //设置初始化状态
}
至此，ORB-SLAM2中传感器为单目相机的初始化过程全部完成。