#include <WAISlamTrackPool.h>

Inheritance diagram for WAISlamTrackPool:

Classes
struct	Params

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

virtual	~WAISlamTrackPool ()

bool	update (cv::Mat &imageGray)

virtual WAITrackingState	getTrackingState ()

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

cv::Mat	getPose ()

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

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

std::vector< WAIMapPoint * >	getMatchedMapPoints ()

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

void	updatePose (WAIFrame &frame)

Private Attributes
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

WAITrackingState	_state

std::mutex	_cameraExtrinsicMutex

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)

Protected Member Functions inherited from WAISlamTools
virtual	~WAISlamTools ()

	WAISlamTools ()

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

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

Detailed Description

Definition at line 6 of file WAISlamTrackPool.h.

Constructor & Destructor Documentation

◆ WAISlamTrackPool()

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

Definition at line 4 of file WAISlamTrackPool.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,
                                               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);
     }
  
     _iniData.initializer = nullptr;
     _cameraExtrinsic     = cv::Mat::eye(4, 4, CV_32F);
  
     _lastFrame = WAIFrame();
 }

◆ ~WAISlamTrackPool()

WAISlamTrackPool::~WAISlamTrackPool ( )

virtual

Definition at line 74 of file WAISlamTrackPool.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();
  
     delete _localMapping;
     delete _loopClosing;
 }

Member Function Documentation

◆ changeIntrinsic()

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

Definition at line 231 of file WAISlamTrackPool.cpp.

 {
     _cameraIntrinsic = intrinsic;
     _distortion      = distortion;
 }

◆ createFrame()

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

private

Definition at line 111 of file WAISlamTrackPool.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 WAISlamTrackPool::drawInfo	(	cv::Mat &	imageBGR,
		float	scale,
		bool	showInitLine,
		bool	showKeyPoints,
		bool	showKeyPointsMatched
	)

Definition at line 243 of file WAISlamTrackPool.cpp.

 {
     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);
     }
 }

◆ getMapPoints()

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

inlinevirtual

Definition at line 61 of file WAISlamTrackPool.h.

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

◆ getMatchedMapPoints()

std::vector< WAIMapPoint * > WAISlamTrackPool::getMatchedMapPoints ( )

Definition at line 268 of file WAISlamTrackPool.cpp.

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

◆ getPose()

cv::Mat WAISlamTrackPool::getPose ( )

Definition at line 237 of file WAISlamTrackPool.cpp.

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

◆ getTrackingState()

virtual WAITrackingState WAISlamTrackPool::getTrackingState ( )

inlinevirtual

Definition at line 51 of file WAISlamTrackPool.h.

51 { return _state; }

◆ update()

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

Definition at line 98 of file WAISlamTrackPool.cpp.

 {
     WAIFrame frame;
     createFrame(frame, imageGray);
  
     // if (_params.ensureKFIntegration)
     //     updatePoseKFIntegration(frame);
     // else
     updatePose(frame);
  
     return (_state == WAITrackingState::TrackingOK);
 }

◆ updatePose()

void WAISlamTrackPool::updatePose ( WAIFrame & frame )

private

Definition at line 145 of file WAISlamTrackPool.cpp.

 {
     switch (_state)
     {
         case WAITrackingState::Initializing:
         {
             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();
                     }
                 }
             }
         }
         break;
  
         case WAITrackingState::TrackingStart:
         {
             _relocFrameCounter++;
             if (_relocFrameCounter > 0)
                 _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;
     }
  
     _lastFrame = WAIFrame(frame);
 }

Member Data Documentation

◆ _cameraExtrinsicMutex

std::mutex WAISlamTrackPool::_cameraExtrinsicMutex

private

Definition at line 85 of file WAISlamTrackPool.h.

◆ _extractor

KPextractor* WAISlamTrackPool::_extractor = nullptr

private

Definition at line 78 of file WAISlamTrackPool.h.

◆ _infoMatchedInliners

int WAISlamTrackPool::_infoMatchedInliners = 0

private

Definition at line 81 of file WAISlamTrackPool.h.

◆ _iniExtractor

KPextractor* WAISlamTrackPool::_iniExtractor = nullptr

private

Definition at line 80 of file WAISlamTrackPool.h.

◆ _lastKeyFrameFrameId

unsigned long WAISlamTrackPool::_lastKeyFrameFrameId = 0

private

Definition at line 77 of file WAISlamTrackPool.h.

◆ _lastRelocFrameId

unsigned long WAISlamTrackPool::_lastRelocFrameId = 0

private

Definition at line 76 of file WAISlamTrackPool.h.

◆ _params

Params WAISlamTrackPool::_params

private

Definition at line 73 of file WAISlamTrackPool.h.

◆ _relocExtractor

KPextractor* WAISlamTrackPool::_relocExtractor = nullptr

private

Definition at line 79 of file WAISlamTrackPool.h.

◆ _relocFrameCounter

unsigned int WAISlamTrackPool::_relocFrameCounter = 0

private

Definition at line 75 of file WAISlamTrackPool.h.

◆ _state

WAITrackingState WAISlamTrackPool::_state

private

Definition at line 83 of file WAISlamTrackPool.h.

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

Classes

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ WAISlamTrackPool()

◆ ~WAISlamTrackPool()

Member Function Documentation

◆ changeIntrinsic()

◆ createFrame()

◆ drawInfo()

◆ getMapPoints()

◆ getMatchedMapPoints()

◆ getPose()

◆ getTrackingState()

◆ update()

◆ updatePose()

Member Data Documentation

◆ _cameraExtrinsicMutex

◆ _extractor

◆ _infoMatchedInliners

◆ _iniExtractor

◆ _lastKeyFrameFrameId

◆ _lastRelocFrameId

◆ _params

◆ _relocExtractor

◆ _relocFrameCounter

◆ _state