WAISlam Class Reference

#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.

{ 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.

{ return _lastFrame.N; }

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.

{ return _localMap.mapPoints; }

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.

{ return _globalMap.get(); }

getMapPointCount()

virtual int WAISlam::getMapPointCount ( )

inlinevirtual

Definition at line 142 of file WAISlam.h.

{ 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.

{ 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.

{ 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.

{ 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:

• WAISlam.h
• WAISlam.cpp