ORB-SLAM 2整理-trackスレッド(四)
14203 ワード
前の3つのブログでは,1つ目はtrackスレッドの全体的な流れを重点的に解析し,2つ目と3つ目は単一初期化過程を解析した.このブログでは、初期化が完了した後の追跡プロセスを分析します.2つの追跡状態、3つの追跡モデルがあります.2つの追跡状態は、それぞれ、非構築図のみを追跡し、bool変数
以上の3つのトラッキングモデルでは、前の2つの基本的な考え方は、現在のフレームの特徴点を参照フレームとマッチングし、マッチング結果に基づいてG 2 Oを用いて最適化することであり、現在のフレームの姿勢初期値の設定とは異なる.3つ目の姿勢追跡モデル,すなわち再配置は辞書Dbowを用いて行う.姿勢最適化部分の内容については,ここでは先に述べず,その後最適化部分を専門に分析する.追跡モデルを用いて現在のフレームの姿勢を推定し最適化した後、次いで、局所地図に対して
キーフレームの作成について.
これで,ORB−SLAM 2に関するtrackスレッドはほぼ整理済みである.本人の能力が限られているため、ブログには間違いがたくさんあるに違いありません.指摘を歓迎します.
mbOnlyTrackingによって区別される.まず紹介するのは3種類の追跡モデルです.//
TrackReferenceKeyFrame() //
TrackWithMotionModel() //
Relocalization() //
bool Tracking::TrackReferenceKeyFrame()
{
// Compute Bag of Words vector
mCurrentFrame.ComputeBoW();
// We perform first an ORB matching with the reference keyframe
// If enough matches are found we setup a PnP solver
ORBmatcher matcher(0.7,true);
vector vpMapPointMatches;
int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
if(nmatches<15)
return false;
mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.mTcw);
Optimizer::PoseOptimization(&mCurrentFrame);
// Discard outliers
int nmatchesMap = 0;
for(int i =0; i(NULL);
mCurrentFrame.mvbOutlier[i]=false;
pMP->mbTrackInView = false;
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
nmatches--;
}
else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
nmatchesMap++;
}
}
return nmatchesMap>=10;
}
bool Tracking::TrackWithMotionModel()
{
ORBmatcher matcher(0.9,true);
// Update last frame pose according to its reference keyframe
// Create "visual odometry" points if in Localization Mode
UpdateLastFrame();
mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast(NULL));
// Project points seen in previous frame
int th;
if(mSensor!=System::STEREO)
th=15;
else
th=7;
int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
// If few matches, uses a wider window search
if(nmatches<20)
{
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast(NULL));
nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR);
}
if(nmatches<20)
return false;
// Optimize frame pose with all matches
Optimizer::PoseOptimization(&mCurrentFrame);
// Discard outliers
int nmatchesMap = 0;
for(int i =0; i(NULL);
mCurrentFrame.mvbOutlier[i]=false;
pMP->mbTrackInView = false;
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
nmatches--;
}
else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
nmatchesMap++;
}
}
if(mbOnlyTracking)
{
mbVO = nmatchesMap<10;
return nmatches>20;
}
return nmatchesMap>=10;
}
bool Tracking::Relocalization()
{
// Compute Bag of Words Vector
mCurrentFrame.ComputeBoW();
// Relocalization is performed when tracking is lost
// Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
vector vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
if(vpCandidateKFs.empty())
return false;
const int nKFs = vpCandidateKFs.size();
// We perform first an ORB matching with each candidate
// If enough matches are found we setup a PnP solver
ORBmatcher matcher(0.75,true);
vector vpPnPsolvers;
vpPnPsolvers.resize(nKFs);
vector > vvpMapPointMatches;
vvpMapPointMatches.resize(nKFs);
vector vbDiscarded;
vbDiscarded.resize(nKFs);
int nCandidates=0;
for(int i=0; iisBad())
vbDiscarded[i] = true;
else
{
int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
if(nmatches<15)
{
vbDiscarded[i] = true;
continue;
}
else
{
PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);
vpPnPsolvers[i] = pSolver;
nCandidates++;
}
}
}
// Alternatively perform some iterations of P4P RANSAC
// Until we found a camera pose supported by enough inliers
bool bMatch = false;
ORBmatcher matcher2(0.9,true);
while(nCandidates>0 && !bMatch)
{
for(int i=0; i vbInliers;
int nInliers;
bool bNoMore;
PnPsolver* pSolver = vpPnPsolvers[i];
cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
// If Ransac reachs max. iterations discard keyframe
if(bNoMore)
{
vbDiscarded[i]=true;
nCandidates--;
}
// If a Camera Pose is computed, optimize
if(!Tcw.empty())
{
Tcw.copyTo(mCurrentFrame.mTcw);
set sFound;
const int np = vbInliers.size();
for(int j=0; j(NULL);
// If few inliers, search by projection in a coarse window and optimize again
if(nGood<50)
{
int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);
if(nadditional+nGood>=50)
{
nGood = Optimizer::PoseOptimization(&mCurrentFrame);
// If many inliers but still not enough, search by projection again in a narrower window
// the camera has been already optimized with many points
if(nGood>30 && nGood<50)
{
sFound.clear();
for(int ip =0; ip=50)
{
nGood = Optimizer::PoseOptimization(&mCurrentFrame);
for(int io =0; io=50)
{
bMatch = true;
break;
}
}
}
}
if(!bMatch)
{
return false;
}
else
{
mnLastRelocFrameId = mCurrentFrame.mnId;
return true;
}
}
以上の3つのトラッキングモデルでは、前の2つの基本的な考え方は、現在のフレームの特徴点を参照フレームとマッチングし、マッチング結果に基づいてG 2 Oを用いて最適化することであり、現在のフレームの姿勢初期値の設定とは異なる.3つ目の姿勢追跡モデル,すなわち再配置は辞書Dbowを用いて行う.姿勢最適化部分の内容については,ここでは先に述べず,その後最適化部分を専門に分析する.追跡モデルを用いて現在のフレームの姿勢を推定し最適化した後、次いで、局所地図に対して
TrackLocalMap()を追跡する.bool Tracking::TrackLocalMap()
{
// We have an estimation of the camera pose and some map points tracked in the frame.
// We retrieve the local map and try to find matches to points in the local map.
UpdateLocalMap();
SearchLocalPoints();
// Optimize Pose
Optimizer::PoseOptimization(&mCurrentFrame);
mnMatchesInliers = 0;
// Update MapPoints Statistics
for(int i=0; iIncreaseFound();
if(!mbOnlyTracking)
{
if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
mnMatchesInliers++;
}
else
mnMatchesInliers++;
}
else if(mSensor==System::STEREO)
mCurrentFrame.mvpMapPoints[i] = static_cast(NULL);
}
}
// Decide if the tracking was succesful
// More restrictive if there was a relocalization recently
if(mCurrentFrame.mnId キーフレームの作成について.
bool Tracking::NeedNewKeyFrame()
{
if(mbOnlyTracking)
return false;
// If Local Mapping is freezed by a Loop Closure do not insert keyframes
if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
return false;
const int nKFs = mpMap->KeyFramesInMap();
// Do not insert keyframes if not enough frames have passed from last relocalisation
if(mCurrentFrame.mnIdmMaxFrames)
return false;
// Tracked MapPoints in the reference keyframe
int nMinObs = 3;
if(nKFs<=2)
nMinObs=2;
int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);
// Local Mapping accept keyframes?
bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();
// Check how many "close" points are being tracked and how many could be potentially created.
int nNonTrackedClose = 0;
int nTrackedClose= 0;
if(mSensor!=System::MONOCULAR)
{
for(int i =0; i0 && mCurrentFrame.mvDepth[i]70);
// Thresholds
float thRefRatio = 0.75f;
if(nKFs<2)
thRefRatio = 0.4f;
if(mSensor==System::MONOCULAR)
thRefRatio = 0.9f;
// Condition 1a: More than "MaxFrames" have passed from last keyframe insertion
const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;
// Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);
//Condition 1c: tracking is weak
const bool c1c = mSensor!=System::MONOCULAR && (mnMatchesInliers15);
if((c1a||c1b||c1c)&&c2)
{
// If the mapping accepts keyframes, insert keyframe.
// Otherwise send a signal to interrupt BA
if(bLocalMappingIdle)
{
return true;
}
else
{
mpLocalMapper->InterruptBA();
if(mSensor!=System::MONOCULAR)
{
if(mpLocalMapper->KeyframesInQueue()<3)
return true;
else
return false;
}
else
return false;
}
}
else
return false;
}
void Tracking::CreateNewKeyFrame()
{
if(!mpLocalMapper->SetNotStop(true))
return;
KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
mpReferenceKF = pKF;
mCurrentFrame.mpReferenceKF = pKF;
if(mSensor!=System::MONOCULAR)
{
mCurrentFrame.UpdatePoseMatrices();
// We sort points by the measured depth by the stereo/RGBD sensor.
// We create all those MapPoints whose depth < mThDepth.
// If there are less than 100 close points we create the 100 closest.
vector > vDepthIdx;
vDepthIdx.reserve(mCurrentFrame.N);
for(int i=0; i0)
{
vDepthIdx.push_back(make_pair(z,i));
}
}
if(!vDepthIdx.empty())
{
sort(vDepthIdx.begin(),vDepthIdx.end());
int nPoints = 0;
for(size_t j=0; jObservations()<1)
{
bCreateNew = true;
mCurrentFrame.mvpMapPoints[i] = static_cast(NULL);
}
if(bCreateNew)
{
cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);
pNewMP->AddObservation(pKF,i);
pKF->AddMapPoint(pNewMP,i);
pNewMP->ComputeDistinctiveDescriptors();
pNewMP->UpdateNormalAndDepth();
mpMap->AddMapPoint(pNewMP);
mCurrentFrame.mvpMapPoints[i]=pNewMP;
nPoints++;
}
else
{
nPoints++;
}
if(vDepthIdx[j].first>mThDepth && nPoints>100)
break;
}
}
}
mpLocalMapper->InsertKeyFrame(pKF);
mpLocalMapper->SetNotStop(false);
mnLastKeyFrameId = mCurrentFrame.mnId;
mpLastKeyFrame = pKF;
}
これで,ORB−SLAM 2に関するtrackスレッドはほぼ整理済みである.本人の能力が限られているため、ブログには間違いがたくさんあるに違いありません.指摘を歓迎します.