#include <WAISlam.h>

Inheritance diagram for WAISlam:

Classes
struct	Params

Public Member Functions
	WAISlam (const cv::Mat &intrinsic, const cv::Mat &distortion, WAIOrbVocabulary voc, KPextractor iniExtractor, KPextractor relocExtractor, KPextractor extractor, std::unique_ptr< WAIMap > globalMap, WAISlam::Params params)

virtual	~WAISlam ()

virtual void	reset ()

void	changeIntrinsic (cv::Mat intrinsic, cv::Mat distortion)

void	createFrame (WAIFrame &frame, cv::Mat &imageGray)

virtual void	updatePose (WAIFrame &frame)

virtual bool	update (cv::Mat &imageGray)

virtual void	updatePoseKFIntegration (WAIFrame &frame)

virtual void	resume ()

virtual bool	isTracking ()

virtual bool	hasStateIdle ()

virtual void	requestStateIdle ()

virtual bool	retainImage ()

void	transformCoords (cv::Mat transform)

std::vector< WAIMapPoint * >	getMatchedMapPoints (WAIFrame *frame)

int	getMatchedCorrespondances (WAIFrame *frame, std::pair< std::vector< cv::Point2f >, std::vector< cv::Point3f >> &matching)

virtual bool	isInitialized ()

virtual WAIMap *	getMap ()

virtual WAIFrame	getLastFrame ()

virtual WAIFrame *	getLastFramePtr ()

virtual std::vector< WAIMapPoint * >	getLocalMapPoints ()

virtual int	getNumKeyFrames ()

virtual std::vector< WAIMapPoint * >	getMapPoints ()

virtual std::vector< WAIKeyFrame * >	getKeyFrames ()

virtual std::string	getPrintableState ()

virtual int	getKeyPointCount ()

virtual int	getKeyFrameCount ()

virtual int	getMapPointCount ()

virtual cv::Mat	getPose ()

virtual void	setCamExrinsicGuess (cv::Mat extrinsicGuess)

virtual void	setMap (std::unique_ptr< WAIMap > globalMap)

virtual WAITrackingState	getTrackingState ()

virtual void	drawInfo (cv::Mat &imageBGR, float scale, bool showInitLine, bool showKeyPoints, bool showKeyPointsMatched)

KPextractor *	getKPextractor ()

int	getMapPointMatchesCount () const

std::string	getLoopCloseStatus ()

int	getLoopCloseCount ()

int	getKeyFramesInLoopCloseQueueCount ()

Protected Member Functions
void	updateState (WAITrackingState state)

bool	finishRequested ()

void	requestFinish ()

bool	isStop ()

bool	isFinished ()

void	flushQueue ()

int	getNextFrame (WAIFrame &frame)

Protected Member Functions inherited from WAISlamTools
virtual	~WAISlamTools ()

	WAISlamTools ()

Static Protected Member Functions
static void	updatePoseThread (WAISlam *ptr)

Static Protected Member Functions inherited from WAISlamTools
static void	countReprojectionOutliers (WAIFrame &frame, unsigned int &m, unsigned int &n, unsigned int &outliers)

Protected Attributes
bool	_requestFinish

bool	_isFinish

bool	_isStop

std::mutex	_stateMutex

WAITrackingState	_state = WAITrackingState::Idle

std::mutex	_cameraExtrinsicMutex

std::mutex	_cameraExtrinsicGuessMutex

std::mutex	_mutexStates

std::mutex	_lastFrameMutex

WAISlam::Params	_params

unsigned int	_relocFrameCounter = 0

unsigned long	_lastRelocFrameId = 0

unsigned long	_lastKeyFrameFrameId = 0

KPextractor *	_extractor = nullptr

KPextractor *	_relocExtractor = nullptr

KPextractor *	_iniExtractor = nullptr

int	_infoMatchedInliners = 0

std::thread *	_poseUpdateThread

std::queue< WAIFrame >	_framesQueue

std::mutex	_frameQueueMutex

Protected Attributes inherited from WAISlamTools
cv::Mat	_distortion

cv::Mat	_cameraIntrinsic

cv::Mat	_cameraExtrinsic

cv::Mat	_cameraExtrinsicGuess

WAIInitializerData	_iniData

WAIFrame	_lastFrame

std::unique_ptr< WAIMap >	_globalMap

LocalMap	_localMap

WAIOrbVocabulary *	_voc = nullptr

cv::Mat	_velocity

bool	_initialized = false

LocalMapping *	_localMapping = nullptr

LoopClosing *	_loopClosing = nullptr

std::thread *	_processNewKeyFrameThread = nullptr

std::vector< std::thread * >	_mappingThreads

std::thread *	_loopClosingThread = nullptr

Additional Inherited Members
Static Public Member Functions inherited from WAISlamTools
static void	drawKeyPointInfo (WAIFrame &frame, cv::Mat &image, float scale)

static void	drawKeyPointMatches (WAIFrame &frame, cv::Mat &image, float scale)

static void	drawInitInfo (WAIInitializerData &iniData, WAIFrame &frame, cv::Mat &imageBGR, float scale)

static bool	initialize (WAIInitializerData &iniData, WAIFrame &frame, WAIOrbVocabulary *voc, LocalMap &localMap, int mapPointsNeeded=100)

static bool	genInitialMap (WAIMap globalMap, LocalMapping localMapper, LoopClosing *loopCloser, LocalMap &localMap)

static bool	oldInitialize (WAIFrame &frame, WAIInitializerData &iniData, WAIMap map, LocalMap &localMap, LocalMapping localMapper, LoopClosing loopCloser, WAIOrbVocabulary voc, int mapPointsNeeded=100)

static int	findFrameFixedMapMatches (WAIFrame &frame, WAIMap *waiMap, std::vector< cv::Point2f > &points2d, std::vector< cv::Point3f > &points3d)

static bool	relocalization (WAIFrame &currentFrame, WAIMap *waiMap, LocalMap &localMap, float minCommonWordFactor, int &inliers, bool minAccScoreFilter=false)

static bool	relocalizationGPS (WAIFrame &currentFrame, WAIMap *waiMap, LocalMap &localMap, cv::Mat locENU, cv::Mat dirENU, float minCommonWordFactor, int &inliers, bool minAccScoreFilter)

static bool	tracking (WAIMap *map, LocalMap &localMap, WAIFrame &frame, WAIFrame &lastFrame, int lastRelocFrameId, cv::Mat &velocity, int &inliers)

static bool	strictTracking (WAIMap *map, LocalMap &localMap, WAIFrame &frame, WAIFrame &lastFrame, int lastRelocFrameId, int &inliers)

static bool	trackLocalMap (LocalMap &localMap, WAIFrame &frame, int lastRelocFrameId, int &inliers)

static void	mapping (WAIMap map, LocalMap &localMap, LocalMapping localMapper, WAIFrame &frame, int inliers, const unsigned long lastRelocFrameId, unsigned long &lastKeyFrameFrameId)

static void	strictMapping (WAIMap map, LocalMap &localMap, LocalMapping localMapper, WAIFrame &frame, int inliers, const unsigned long lastRelocFrameId, unsigned long &lastKeyFrameFrameId)

static void	motionModel (WAIFrame &frame, WAIFrame &lastFrame, cv::Mat &velocity, cv::Mat &pose)

static bool	trackReferenceKeyFrame (LocalMap &map, WAIFrame &lastFrame, WAIFrame &frame)

static bool	strictTrackReferenceKeyFrame (LocalMap &map, WAIFrame &lastFrame, WAIFrame &frame)

static bool	trackWithMotionModel (cv::Mat velocity, WAIFrame &previousFrame, WAIFrame &frame)

static void	updateLocalMap (WAIFrame &frame, LocalMap &localMap)

static int	trackLocalMapPoints (LocalMap &localMap, int lastRelocFrameId, WAIFrame &frame)

static bool	needNewKeyFrame (WAIMap globalMap, LocalMap &localMap, LocalMapping localMapper, WAIFrame &frame, int nInliners, const unsigned long lastRelocFrameId, const unsigned long lastKeyFrameFrameId)

static bool	strictNeedNewKeyFrame (WAIMap map, LocalMap &localMap, LocalMapping localMapper, WAIFrame &frame, int nInliers, const unsigned long lastRelocFrameId, const unsigned long lastKeyFrameFrameId)

static void	createNewKeyFrame (LocalMapping localMapper, LocalMap &localMap, WAIMap globalMap, WAIFrame &frame, unsigned long &lastKeyFrameFrameId)

static WAIFrame	createMarkerFrame (std::string markerFile, KPextractor markerExtractor, const cv::Mat &markerCameraIntrinsic, WAIOrbVocabulary voc)

static bool	findMarkerHomography (WAIFrame &markerFrame, WAIKeyFrame *kfCand, cv::Mat &homography, int minMatches)

static bool	doMarkerMapPreprocessing (std::string markerFile, cv::Mat &nodeTransform, float markerWidthInM, KPextractor markerExtractor, WAIMap map, const cv::Mat &markerCameraIntrinsic, WAIOrbVocabulary *voc)

static bool	detectCycle (WAIKeyFrame kf, std::set< WAIKeyFrame > &visitedNode)

static bool	checkKFConnectionsTree (WAIMap *map)

Detailed Description

Definition at line 26 of file WAISlam.h.

Constructor & Destructor Documentation

◆ WAISlam()

WAISlam::WAISlam	(	const cv::Mat &	intrinsic,
		const cv::Mat &	distortion,
		WAIOrbVocabulary *	voc,
		KPextractor *	iniExtractor,
		KPextractor *	relocExtractor,
		KPextractor *	extractor,
		std::unique_ptr< WAIMap >	globalMap,
		WAISlam::Params	params
	)

Definition at line 15 of file WAISlam.cpp.

 {
     _iniData.initializer = nullptr;
     _params              = params;
  
     WAIFrame::nNextId               = 0;
     WAIFrame::mbInitialComputations = true;
  
     _lastKeyFrameFrameId = 0;
     _lastRelocFrameId    = 0;
  
     _distortion      = distortion.clone();
     _cameraIntrinsic = intrinsic.clone();
     _voc             = voc;
  
     _extractor      = extractor;
     _relocExtractor = relocExtractor;
     _iniExtractor   = iniExtractor;
  
     if (_iniExtractor == nullptr)
         _iniExtractor = _extractor;
     if (_relocExtractor == nullptr)
         _relocExtractor = _extractor;
  
     if (globalMap == nullptr)
     {
         WAIKeyFrameDB* kfDB = new WAIKeyFrameDB(voc);
         _globalMap          = std::make_unique<WAIMap>(kfDB);
         _state              = WAITrackingState::Initializing;
  
         WAIKeyFrame::nNextId = 0;
         WAIMapPoint::nNextId = 0;
     }
     else
     {
         _globalMap   = std::move(globalMap);
         _state       = WAITrackingState::TrackingLost;
         _initialized = true;
     }
  
     _localMapping = new ORB_SLAM2::LocalMapping(_globalMap.get(),
                                                 _voc,
                                                 params.cullRedundantPerc);
     _loopClosing  = new ORB_SLAM2::LoopClosing(_globalMap.get(),
                                               _voc,
                                               _params.minCommonWordFactor,
                                               false,
                                               false);
  
     _localMapping->SetLoopCloser(_loopClosing);
     _loopClosing->SetLocalMapper(_localMapping);
  
     if (!_params.onlyTracking && !_params.serial)
     {
         _mappingThreads.push_back(new std::thread(&LocalMapping::Run, _localMapping));
         _loopClosingThread = new std::thread(&LoopClosing::Run, _loopClosing);
     }
  
 #if MULTI_THREAD_FRAME_PROCESSING
     _poseUpdateThread = new std::thread(updatePoseThread, this);
     _isFinish         = false;
     _isStop           = false;
     _requestFinish    = false;
 #endif
  
     _iniData.initializer = nullptr;
     _cameraExtrinsic     = cv::Mat::eye(4, 4, CV_32F);
  
     _lastFrame = WAIFrame();
 }

◆ ~WAISlam()

WAISlam::~WAISlam ( )

virtual

Definition at line 93 of file WAISlam.cpp.

 {
     if (!_params.serial)
     {
         _localMapping->RequestFinish();
         _loopClosing->RequestFinish();
     }
  
     // Wait until all thread have effectively stopped
     if (_processNewKeyFrameThread)
         _processNewKeyFrameThread->join();
  
     for (std::thread* t : _mappingThreads) { t->join(); }
  
     if (_loopClosingThread)
         _loopClosingThread->join();
  
 #if MULTI_THREAD_FRAME_PROCESSING
     requestFinish();
     _poseUpdateThread->join();
     delete _poseUpdateThread;
     _poseUpdateThread = nullptr;
 #endif
  
     delete _localMapping;
     delete _loopClosing;
 }

Member Function Documentation

◆ changeIntrinsic()

void WAISlam::changeIntrinsic	(	cv::Mat	intrinsic,
		cv::Mat	distortion
	)

Definition at line 156 of file WAISlam.cpp.

 {
     _cameraIntrinsic = intrinsic;
     _distortion      = distortion;
 }

◆ createFrame()

void WAISlam::createFrame	(	WAIFrame &	frame,
		cv::Mat &	imageGray
	)

Definition at line 162 of file WAISlam.cpp.

 {
     switch (getTrackingState())
     {
         case WAITrackingState::Initializing:
             frame = WAIFrame(imageGray,
                              0.0,
                              _iniExtractor,
                              _cameraIntrinsic,
                              _distortion,
                              _voc,
                              _params.retainImg);
             break;
         case WAITrackingState::TrackingLost:
         case WAITrackingState::TrackingStart:
             frame = WAIFrame(imageGray,
                              0.0,
                              _relocExtractor,
                              _cameraIntrinsic,
                              _distortion,
                              _voc,
                              _params.retainImg);
             break;
         default:
             frame = WAIFrame(imageGray,
                              0.0,
                              _extractor,
                              _cameraIntrinsic,
                              _distortion,
                              _voc,
                              _params.retainImg);
     }
 }

◆ drawInfo()

void WAISlam::drawInfo	(	cv::Mat &	imageBGR,
		float	scale,
		bool	showInitLine,
		bool	showKeyPoints,
		bool	showKeyPointsMatched
	)

virtual

Definition at line 488 of file WAISlam.cpp.

 {
     std::unique_lock<std::mutex> lock(_lastFrameMutex);
     std::unique_lock<std::mutex> lock2(_stateMutex);
     if (_state == WAITrackingState::Initializing)
     {
         if (showInitLine)
             drawInitInfo(_iniData, _lastFrame, imageBGR, scale);
     }
     else if (_state == WAITrackingState::TrackingOK)
     {
         if (showKeyPoints)
             drawKeyPointInfo(_lastFrame, imageBGR, scale);
         if (showKeyPointsMatched)
             drawKeyPointMatches(_lastFrame, imageBGR, scale);
     }
     else if (_state == WAITrackingState::TrackingLost)
     {
         if (showKeyPoints)
             drawKeyPointInfo(_lastFrame, imageBGR, scale);
     }
 }

◆ finishRequested()

bool WAISlam::finishRequested ( )

protected

Definition at line 231 of file WAISlam.cpp.

 {
     std::unique_lock<std::mutex> lock(_stateMutex);
     return _requestFinish;
 }

◆ flushQueue()

void WAISlam::flushQueue ( )

protected

Definition at line 197 of file WAISlam.cpp.

 {
     std::unique_lock<std::mutex> lock(_lastFrameMutex);
     while (!_framesQueue.empty())
     {
         _framesQueue.pop();
     }
 }

◆ getKeyFrameCount()

virtual int WAISlam::getKeyFrameCount ( )

inlinevirtual

Definition at line 141 of file WAISlam.h.

141 { return (int)_globalMap->KeyFramesInMap(); }

◆ getKeyFrames()

virtual std::vector<WAIKeyFrame*> WAISlam::getKeyFrames ( )

inlinevirtual

Definition at line 106 of file WAISlam.h.

     {
         if (_globalMap != nullptr)
             return _globalMap->GetAllKeyFrames();
         return std::vector<WAIKeyFrame*>();
     }

◆ getKeyFramesInLoopCloseQueueCount()

int WAISlam::getKeyFramesInLoopCloseQueueCount ( )

Definition at line 639 of file WAISlam.cpp.

 {
     return _loopClosing->numOfKfsInQueue();
 }

◆ getKeyPointCount()

virtual int WAISlam::getKeyPointCount ( )

inlinevirtual

Definition at line 140 of file WAISlam.h.

140 { return _lastFrame.N; }

WAIFrame::N

int N

Definition: WAIFrame.h:123

◆ getKPextractor()

KPextractor* WAISlam::getKPextractor ( )

inline

Definition at line 157 of file WAISlam.h.

     {
         return _extractor;
     };

◆ getLastFrame()

WAIFrame WAISlam::getLastFrame ( )

virtual

Definition at line 459 of file WAISlam.cpp.

 {
     std::unique_lock<std::mutex> lock(_lastFrameMutex);
     return _lastFrame;
 }

◆ getLastFramePtr()

WAIFrame * WAISlam::getLastFramePtr ( )

virtual

Definition at line 465 of file WAISlam.cpp.

 {
     std::unique_lock<std::mutex> lock(_lastFrameMutex);
     return &_lastFrame;
 }

◆ getLocalMapPoints()

virtual std::vector<WAIMapPoint*> WAISlam::getLocalMapPoints ( )

inlinevirtual

Definition at line 96 of file WAISlam.h.

96 { return _localMap.mapPoints; }

WAISlamTools::_localMap

LocalMap _localMap

Definition: WAISlamTools.h:209

◆ getLoopCloseCount()

int WAISlam::getLoopCloseCount ( )

Definition at line 634 of file WAISlam.cpp.

 {
     return _globalMap->getNumLoopClosings();
 }

◆ getLoopCloseStatus()

std::string WAISlam::getLoopCloseStatus ( )

Definition at line 629 of file WAISlam.cpp.

 {
     return _loopClosing->getStatusString();
 }

◆ getMap()

virtual WAIMap* WAISlam::getMap ( )

inlinevirtual

Definition at line 93 of file WAISlam.h.

93 { return _globalMap.get(); }

◆ getMapPointCount()

virtual int WAISlam::getMapPointCount ( )

inlinevirtual

Definition at line 142 of file WAISlam.h.

142 { return (int)_globalMap->MapPointsInMap(); }

◆ getMapPointMatchesCount()

int WAISlam::getMapPointMatchesCount ( ) const

Definition at line 624 of file WAISlam.cpp.

 {
     return _infoMatchedInliners;
 }

◆ getMapPoints()

virtual std::vector<WAIMapPoint*> WAISlam::getMapPoints ( )

inlinevirtual

Definition at line 99 of file WAISlam.h.

     {
         if (_globalMap != nullptr)
             return _globalMap->GetAllMapPoints();
         return std::vector<WAIMapPoint*>();
     }

◆ getMatchedCorrespondances()

int WAISlam::getMatchedCorrespondances	(	WAIFrame *	frame,
		std::pair< std::vector< cv::Point2f >, std::vector< cv::Point3f >> &	matching
	)

Definition at line 531 of file WAISlam.cpp.

 {
     for (int i = 0; i < frame->N; i++)
     {
         WAIMapPoint* mp = frame->mvpMapPoints[i];
         if (mp)
         {
             if (mp->isFixed())
             {
                 WAI::V3 v = mp->worldPosVec();
                 matching.first.push_back(frame->mvKeysUn[i].pt);
                 matching.second.push_back(cv::Point3f(v.x, v.y, v.z));
             }
         }
     }
     return (int)matching.first.size();
 }

◆ getMatchedMapPoints()

std::vector< WAIMapPoint * > WAISlam::getMatchedMapPoints ( WAIFrame * frame )

Definition at line 515 of file WAISlam.cpp.

 {
     std::vector<WAIMapPoint*> result;
  
     for (int i = 0; i < frame->N; i++)
     {
         if (frame->mvpMapPoints[i])
         {
             if (frame->mvpMapPoints[i]->Observations() > 0)
                 result.push_back(frame->mvpMapPoints[i]);
         }
     }
  
     return result;
 }

◆ getNextFrame()

int WAISlam::getNextFrame ( WAIFrame & frame )

protected

Definition at line 212 of file WAISlam.cpp.

 {
     int                          nbFrameInQueue;
     std::unique_lock<std::mutex> lock(_frameQueueMutex);
     nbFrameInQueue = (int)_framesQueue.size();
     if (nbFrameInQueue == 0)
         return 0;
  
     frame = _framesQueue.front();
     _framesQueue.pop();
     return nbFrameInQueue;
 }

◆ getNumKeyFrames()

virtual int WAISlam::getNumKeyFrames ( )

inlinevirtual

Definition at line 97 of file WAISlam.h.

97 { return (int)_globalMap->KeyFramesInMap(); }

◆ getPose()

cv::Mat WAISlam::getPose ( )

virtual

Definition at line 551 of file WAISlam.cpp.

 {
     std::unique_lock<std::mutex> lock(_cameraExtrinsicMutex);
     return _cameraExtrinsic;
 }

◆ getPrintableState()

virtual std::string WAISlam::getPrintableState ( )

inlinevirtual

Definition at line 113 of file WAISlam.h.

     {
         switch (_state)
         {
             case WAITrackingState::Idle:
                 return std::string("TrackingState_Idle\n");
                 break;
             case WAITrackingState::Initializing:
                 return std::string("Initializing");
                 break;
             case WAITrackingState::None:
                 return std::string("None");
                 break;
             case WAITrackingState::TrackingLost:
                 return std::string("TrackingLost");
                 break;
             case WAITrackingState::TrackingOK:
                 return std::string("TrackingOK");
                 break;
             case WAITrackingState::TrackingStart:
                 return std::string("TrackingStart");
                 break;
             default:
                 return std::string("");
         }
     }

◆ getTrackingState()

virtual WAITrackingState WAISlam::getTrackingState ( )

inlinevirtual

Definition at line 149 of file WAISlam.h.

149 { return _state; }

◆ hasStateIdle()

bool WAISlam::hasStateIdle ( )

virtual

Definition at line 579 of file WAISlam.cpp.

 {
     std::unique_lock<std::mutex> guard(_mutexStates);
     return (_state == WAITrackingState::Idle);
 }

◆ isFinished()

bool WAISlam::isFinished ( )

protected

Definition at line 237 of file WAISlam.cpp.

 {
     std::unique_lock<std::mutex> lock(_stateMutex);
     return _isFinish;
 }

◆ isInitialized()

virtual bool WAISlam::isInitialized ( )

inlinevirtual

Definition at line 92 of file WAISlam.h.

92 { return _initialized; }

◆ isStop()

bool WAISlam::isStop ( )

protected

Definition at line 243 of file WAISlam.cpp.

 {
     std::unique_lock<std::mutex> lock(_stateMutex);
     return _isStop;
 }

◆ isTracking()

bool WAISlam::isTracking ( )

virtual

Definition at line 585 of file WAISlam.cpp.

 {
     std::unique_lock<std::mutex> guard(_mutexStates);
     return _state == WAITrackingState::TrackingOK;
 }

◆ requestFinish()

void WAISlam::requestFinish ( )

protected

Definition at line 225 of file WAISlam.cpp.

 {
     std::unique_lock<std::mutex> lock(_stateMutex);
     _requestFinish = true;
 }

◆ requestStateIdle()

void WAISlam::requestStateIdle ( )

virtual

Definition at line 563 of file WAISlam.cpp.

 {
     if (!(_params.onlyTracking || _params.serial))
     {
         std::unique_lock<std::mutex> guard(_mutexStates);
         _localMapping->RequestPause();
         while (!_localMapping->isPaused())
         {
             std::this_thread::sleep_for(std::chrono::microseconds(10));
         }
         std::cout << "localMapping is stopped" << std::endl;
     }
  
     _state = WAITrackingState::Idle;
 }

◆ reset()

void WAISlam::reset ( )

virtual

Definition at line 121 of file WAISlam.cpp.

 {
     if (!_params.serial)
     {
         _localMapping->RequestReset();
         _loopClosing->RequestReset();
     }
  
 #if MULTI_THREAD_FRAME_PROCESSING
     requestFinish();
     _poseUpdateThread->join();
 #endif
  
     _globalMap->clear();
     _localMap.keyFrames.clear();
     _localMap.mapPoints.clear();
     _localMap.refKF = nullptr;
  
     _lastKeyFrameFrameId = 0;
     _lastRelocFrameId    = 0;
  
 #if MULTI_THREAD_FRAME_PROCESSING
     _poseUpdateThread = new std::thread(updatePoseThread, this);
     _isFinish         = false;
     _isStop           = false;
     _requestFinish    = false;
 #endif
  
     WAIKeyFrame::nNextId            = 0;
     WAIFrame::nNextId               = 0;
     WAIFrame::mbInitialComputations = true;
     WAIMapPoint::nNextId            = 0;
     _state                          = WAITrackingState::Initializing;
 }

◆ resume()

void WAISlam::resume ( )

virtual

Definition at line 249 of file WAISlam.cpp.

 {
     std::unique_lock<std::mutex> lock(_stateMutex);
     _isStop = false;
     _localMapping->RequestContinue();
     _state = WAITrackingState::TrackingLost;
 }

◆ retainImage()

bool WAISlam::retainImage ( )

virtual

Definition at line 591 of file WAISlam.cpp.

 {
     return false;
 }

◆ setCamExrinsicGuess()

void WAISlam::setCamExrinsicGuess ( cv::Mat extrinsicGuess )

virtual

Definition at line 557 of file WAISlam.cpp.

 {
     std::unique_lock<std::mutex> lock(_cameraExtrinsicGuessMutex);
     _cameraExtrinsicGuess = extrinsicGuess;
 }

◆ setMap()

void WAISlam::setMap ( std::unique_ptr< WAIMap > globalMap )

virtual

Definition at line 615 of file WAISlam.cpp.

 {
     requestStateIdle();
     reset();
     _globalMap   = std::move(globalMap);
     _initialized = true;
     resume();
 }

◆ transformCoords()

void WAISlam::transformCoords ( cv::Mat transform )

Definition at line 596 of file WAISlam.cpp.

 {
     if (_loopClosingThread != nullptr)
     {
         _localMapping->RequestPause();
         while (!_localMapping->isPaused() && !_localMapping->isFinished())
         {
             std::this_thread::sleep_for(std::chrono::seconds(1));
         }
     }
  
     WAIMap* map = _globalMap.get();
  
     map->transform(transform);
  
     _initialized = true;
     _localMapping->RequestContinue();
 }

◆ update()

bool WAISlam::update ( cv::Mat & imageGray )

virtual

Definition at line 471 of file WAISlam.cpp.

 {
     WAIFrame frame;
     createFrame(frame, imageGray);
  
 #if MULTI_THREAD_FRAME_PROCESSING
     std::unique_lock<std::mutex> lock(_frameQueueMutex);
     _framesQueue.push(frame);
 #else
     if (_params.ensureKFIntegration)
         updatePoseKFIntegration(frame);
     else
         updatePose(frame);
 #endif
     return isTracking();
 }

◆ updatePose()

void WAISlam::updatePose ( WAIFrame & frame )

virtual

Definition at line 291 of file WAISlam.cpp.

 {
     std::unique_lock<std::mutex> guard(_mutexStates);
  
     switch (_state)
     {
         case WAITrackingState::Initializing:
         {
 #if 0
             bool ok = oldInitialize(frame, _iniData, _globalMap.get(), _localMap, _localMapping, _loopClosing, _voc);
             if (ok)
             {
                 _lastKeyFrameFrameId = frame.mnId;
                 _lastRelocFrameId    = 0;
                 _state               = WAI::TrackingOK;
                 _initialized         = true;
             }
 #else
             if (initialize(_iniData, frame, _voc, _localMap))
             {
                 if (genInitialMap(_globalMap.get(), _localMapping, _loopClosing, _localMap))
                 {
                     _localMapping->InsertKeyFrame(_localMap.keyFrames[0]);
                     _localMapping->InsertKeyFrame(_localMap.keyFrames[1]);
                     _lastKeyFrameFrameId = frame.mnId;
                     _lastRelocFrameId    = 0;
                     _state               = WAITrackingState::TrackingOK;
                     _initialized         = true;
                     if (_params.serial)
                     {
                         _localMapping->RunOnce();
                         _localMapping->RunOnce();
                     }
                 }
             }
 #endif
         }
         break;
         case WAITrackingState::TrackingStart:
         {
             _relocFrameCounter++;
             if (_relocFrameCounter > 30)
                 _state = WAITrackingState::TrackingOK;
         }
         case WAITrackingState::TrackingOK:
         {
             int inliers;
             if (tracking(_globalMap.get(),
                          _localMap,
                          frame,
                          _lastFrame,
                          (int)_lastRelocFrameId,
                          _velocity,
                          inliers))
             {
                 std::unique_lock<std::mutex> lock(_cameraExtrinsicMutex);
                 motionModel(frame,
                             _lastFrame,
                             _velocity,
                             _cameraExtrinsic);
                 lock.unlock();
                 mapping(_globalMap.get(),
                         _localMap,
                         _localMapping,
                         frame,
                         inliers,
                         _lastRelocFrameId,
                         _lastKeyFrameFrameId);
                 if (_params.serial)
                 {
                     _localMapping->RunOnce();
                     _loopClosing->RunOnce();
                 }
                 _infoMatchedInliners = inliers;
             }
             else
             {
                 _state = WAITrackingState::TrackingLost;
             }
         }
         break;
         case WAITrackingState::TrackingLost:
         {
             int inliers;
             if (relocalization(frame,
                                _globalMap.get(),
                                _localMap,
                                _params.minCommonWordFactor,
                                inliers,
                                _params.minAccScoreFilter))
             {
                 _relocFrameCounter = 0;
                 _lastRelocFrameId  = frame.mnId;
                 _velocity          = cv::Mat();
                 _state             = WAITrackingState::TrackingStart;
             }
         }
         break;
     }
  
     std::unique_lock<std::mutex> lock(_lastFrameMutex);
     _lastFrame = WAIFrame(frame);
 }

◆ updatePoseKFIntegration()

void WAISlam::updatePoseKFIntegration ( WAIFrame & frame )

virtual

Definition at line 395 of file WAISlam.cpp.

 {
     switch (_state)
     {
         case WAITrackingState::TrackingOK:
         {
             int inliers;
             if (tracking(_globalMap.get(),
                          _localMap,
                          frame,
                          _lastFrame,
                          (int)_lastRelocFrameId,
                          _velocity,
                          inliers))
             {
                 std::unique_lock<std::mutex> lock(_cameraExtrinsicMutex);
                 motionModel(frame,
                             _lastFrame,
                             _velocity,
                             _cameraExtrinsic);
                 lock.unlock();
                 strictMapping(_globalMap.get(),
                               _localMap,
                               _localMapping,
                               frame,
                               inliers,
                               _lastRelocFrameId,
                               _lastKeyFrameFrameId);
                 if (_params.serial)
                 {
                     _localMapping->RunOnce();
                     _loopClosing->RunOnce();
                 }
                 _infoMatchedInliners = inliers;
             }
             else
             {
                 _state = WAITrackingState::TrackingLost;
             }
         }
         break;
         case WAITrackingState::TrackingLost:
         {
             int inliers;
             if (relocalization(frame,
                                _globalMap.get(),
                                _localMap,
                                _params.minCommonWordFactor,
                                inliers,
                                _params.minAccScoreFilter))
             {
                 _lastRelocFrameId    = frame.mnId;
                 _velocity            = cv::Mat();
                 _state               = WAITrackingState::TrackingOK;
                 _infoMatchedInliners = inliers;
             }
         }
         break;
     }
  
     std::unique_lock<std::mutex> lock(_lastFrameMutex);
     _lastFrame = WAIFrame(frame);
 }

◆ updatePoseThread()

void WAISlam::updatePoseThread ( WAISlam * ptr )

staticprotected

Definition at line 257 of file WAISlam.cpp.

 {
     while (1)
     {
         WAIFrame f;
         while (ptr->getNextFrame(f) && !ptr->finishRequested())
         {
             if (ptr->_params.ensureKFIntegration)
                 ptr->updatePoseKFIntegration(f);
             else
                 ptr->updatePose(f);
         }
  
         if (ptr->finishRequested())
         {
             std::unique_lock<std::mutex> lock(ptr->_frameQueueMutex);
  
             while (!ptr->_framesQueue.empty())
                 ptr->_framesQueue.pop();
  
             break;
         }
  
         while (ptr->isStop() && !ptr->isFinished() && !ptr->finishRequested())
         {
             std::this_thread::sleep_for(25ms);
         }
     }
  
     std::unique_lock<std::mutex> lock(ptr->_stateMutex);
     ptr->_requestFinish = false;
     ptr->_isFinish      = true;
 }

◆ updateState()

void WAISlam::updateState ( WAITrackingState state )

protected

Definition at line 206 of file WAISlam.cpp.

 {
     std::unique_lock<std::mutex> lock(_mutexStates);
     _state = state;
 }

Member Data Documentation

◆ _cameraExtrinsicGuessMutex

std::mutex WAISlam::_cameraExtrinsicGuessMutex

protected

Definition at line 187 of file WAISlam.h.

◆ _cameraExtrinsicMutex

std::mutex WAISlam::_cameraExtrinsicMutex

protected

Definition at line 186 of file WAISlam.h.

◆ _extractor

KPextractor* WAISlam::_extractor = nullptr

protected

Definition at line 196 of file WAISlam.h.

◆ _frameQueueMutex

std::mutex WAISlam::_frameQueueMutex

protected

Definition at line 202 of file WAISlam.h.

◆ _framesQueue

std::queue<WAIFrame> WAISlam::_framesQueue

protected

Definition at line 201 of file WAISlam.h.

◆ _infoMatchedInliners

int WAISlam::_infoMatchedInliners = 0

protected

Definition at line 199 of file WAISlam.h.

◆ _iniExtractor

KPextractor* WAISlam::_iniExtractor = nullptr

protected

Definition at line 198 of file WAISlam.h.

◆ _isFinish

bool WAISlam::_isFinish

protected

Definition at line 174 of file WAISlam.h.

◆ _isStop

bool WAISlam::_isStop

protected

Definition at line 175 of file WAISlam.h.

◆ _lastFrameMutex

std::mutex WAISlam::_lastFrameMutex

protected

Definition at line 189 of file WAISlam.h.

◆ _lastKeyFrameFrameId

unsigned long WAISlam::_lastKeyFrameFrameId = 0

protected

Definition at line 195 of file WAISlam.h.

◆ _lastRelocFrameId

unsigned long WAISlam::_lastRelocFrameId = 0

protected

Definition at line 194 of file WAISlam.h.

◆ _mutexStates

std::mutex WAISlam::_mutexStates

protected

Definition at line 188 of file WAISlam.h.

◆ _params

WAISlam::Params WAISlam::_params

protected

Definition at line 191 of file WAISlam.h.

◆ _poseUpdateThread

std::thread* WAISlam::_poseUpdateThread

protected

Definition at line 200 of file WAISlam.h.

◆ _relocExtractor

KPextractor* WAISlam::_relocExtractor = nullptr

protected

Definition at line 197 of file WAISlam.h.

◆ _relocFrameCounter

unsigned int WAISlam::_relocFrameCounter = 0

protected

Definition at line 193 of file WAISlam.h.

◆ _requestFinish

bool WAISlam::_requestFinish

protected

Definition at line 173 of file WAISlam.h.

◆ _state

WAITrackingState WAISlam::_state = WAITrackingState::Idle

protected

Definition at line 185 of file WAISlam.h.

◆ _stateMutex

std::mutex WAISlam::_stateMutex

protected

Definition at line 176 of file WAISlam.h.

The documentation for this class was generated from the following files:

Classes

Public Member Functions

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ WAISlam()

◆ ~WAISlam()

Member Function Documentation

◆ changeIntrinsic()

◆ createFrame()

◆ drawInfo()

◆ finishRequested()

◆ flushQueue()

◆ getKeyFrameCount()

◆ getKeyFrames()

◆ getKeyFramesInLoopCloseQueueCount()

◆ getKeyPointCount()

◆ getKPextractor()

◆ getLastFrame()

◆ getLastFramePtr()

◆ getLocalMapPoints()

◆ getLoopCloseCount()

◆ getLoopCloseStatus()

◆ getMap()

◆ getMapPointCount()

◆ getMapPointMatchesCount()

◆ getMapPoints()

◆ getMatchedCorrespondances()

◆ getMatchedMapPoints()

◆ getNextFrame()

◆ getNumKeyFrames()

◆ getPose()

◆ getPrintableState()

◆ getTrackingState()

◆ hasStateIdle()

◆ isFinished()

◆ isInitialized()

◆ isStop()

◆ isTracking()

◆ requestFinish()

◆ requestStateIdle()

◆ reset()

◆ resume()

◆ retainImage()

◆ setCamExrinsicGuess()

◆ setMap()

◆ transformCoords()

◆ update()

◆ updatePose()

◆ updatePoseKFIntegration()

◆ updatePoseThread()

◆ updateState()

Member Data Documentation

◆ _cameraExtrinsicGuessMutex

◆ _cameraExtrinsicMutex

◆ _extractor

◆ _frameQueueMutex

◆ _framesQueue

◆ _infoMatchedInliners

◆ _iniExtractor

◆ _isFinish

◆ _isStop

◆ _lastFrameMutex

◆ _lastKeyFrameFrameId

◆ _lastRelocFrameId

◆ _mutexStates

◆ _params

◆ _poseUpdateThread

◆ _relocExtractor

◆ _relocFrameCounter

◆ _requestFinish

◆ _state

◆ _stateMutex