資源簡介
這是YGZ立體慣性SLAM,一種立體慣性VO代碼。它專為立體聲和立體聲慣性傳感器模塊而設計,如vi-sensor。它使用LK光流作為前端,并使用滑動窗口束調整作為后端。隨意在數據集和您自己的傳感器中嘗試它。高博寫的基于LK光流前端的源碼,自行添加了ROS節點
代碼片段和文件信息
#include?“ygz/Feature.h“
#include?“ygz/MapPoint.h“
#include?“ygz/frame.h“
#include?“ygz/BackendSlidingWindowG2O.h“
#include?“ygz/Tracker.h“
#include?“ygz/ORBMatcher.h“
#include?“ygz/Camera.h“
//?g2o?related
#include?“ygz/G2OTypes.h“
#include?
#include?
#include?
#include?
#include?
namespace?ygz?{
????bool?BackendSlidingWindowG2O::IsBusy()?{
????????unique_lock?lock(mMutexAccept);
????????return?!mbAcceptKeyframes;
????}
????void?BackendSlidingWindowG2O::MainLoop()?{
????????mbFinished?=?false;
????????while?(1)?{
????????????//?Tracking?will?see?that?Local?Mapping?is?busy
????????????SetAcceptKeyframes(false);
????????????//?Check?if?there?are?keyframes?in?the?queue
????????????if?(CheckNewKeyframes())?{
????????????????//?BoW?conversion?and?insertion?in?Map
????????????????mbAbortBA?=?false;
????????????????LOG(INFO)?<“Process?new?KF“?<????????????????ProcessNewKeyframe();
????????????????LOG(INFO)?<“Process?new?KF?done.“?<????????????}?else?if?(Stop())?{
????????????????//?Safe?area?to?stop
????????????????while?(isStopped()?&&?!CheckFinish())?{
????????????????????usleep(3000);
????????????????}
????????????????if?(CheckFinish())
????????????????????break;
????????????}
????????????ResetIfRequested();
????????????//?Tracking?will?see?that?Local?Mapping?is?busy
????????????SetAcceptKeyframes(true);
????????????if?(CheckFinish())
????????????????break;
????????????usleep(3000);
????????}
????????SetFinish();
????}
????void?BackendSlidingWindowG2O::ProcessNewKeyframe()?{
????????{
????????????unique_lock?lock(mMutexNewKFs);
????????????mpCurrent?=?mpNewKFs.front();
????????????mpNewKFs.pop_front();
????????}
????????{
????????????unique_lock?lockKF(mMutexKFs);
????????????mpKFs.push_back(mpCurrent);
????????}
????????//?將雙目匹配出來的,且沒有關聯地圖點的那些點,為它們創建新的地圖點
????????mpCurrent->AssignFeaturesToGrid();
????????int?cntSetGood?=?0?cntImmature?=?0;
????????{
????????????unique_lock?lockMps(mMutexPoints);
????????????unique_lock?lock(mpCurrent->mMutexFeature);
????????????for?(size_t?i?=?0;?i?mFeaturesLeft.size();?i++)?{
????????????????shared_ptr?feat?=?mpCurrent->mFeaturesLeft[i];
????????????????if?(feat?==?nullptr)
????????????????????continue;
????????????????if?(feat->mfInvDepth?>?0?&&?feat->mpPoint?==?nullptr)?{
????????????????????//?距離過近或過遠都不可靠
????????????????????if?(feat->mfInvDepth?????????????????????????feat->mfInvDepth?>?setting::maxNewMapPointInvD)
????????????????????????continue;
????????????????????//?create?a?new?map?point
????????????????????shared_ptr?pNewMP(new?MapPoint(mpCurrent?i));
????????????????????pNewMP->AddObservation(mpCurrent?i);
????????????????????feat->mpPoint?=?pNewMP;
????????????????????pNewMP->Com
評論
共有 條評論