SLProject  4.2.000
A platform independent 3D computer graphics framework for desktop OS, Android, iOS and online in web browsers
WAIKeyFrame Class Reference

AR Keyframe node class. More...

#include <WAIKeyFrame.h>

Public Member Functions

 WAIKeyFrame (const cv::Mat &Tcw, unsigned long id, bool fixKF, float fx, float fy, float cx, float cy, size_t N, const std::vector< cv::KeyPoint > &vKeysUn, const cv::Mat &descriptors, WAIOrbVocabulary *vocabulary, int nScaleLevels, float fScaleFactor, const std::vector< float > &vScaleFactors, const std::vector< float > &vLevelSigma2, const std::vector< float > &vInvLevelSigma2, int nMinX, int nMinY, int nMaxX, int nMaxY, const cv::Mat &KB)
 keyframe generation during map loading More...
 
 WAIKeyFrame (WAIFrame &F, bool retainImg=true)
 keyframe generation from frame More...
 
void SetPose (const cv::Mat &Tcw)
 
cv::Mat GetPose ()
 
cv::Mat GetPoseInverse ()
 
cv::Mat GetCameraCenter ()
 
cv::Mat GetRotation ()
 
cv::Mat GetTranslation ()
 
void ComputeBoW (WAIOrbVocabulary *vocabulary)
 
void SetBowVector (WAIBowVector &bow)
 
void AddConnection (WAIKeyFrame *pKF, int weight)
 
void EraseConnection (WAIKeyFrame *pKF)
 
void FindAndUpdateConnections (bool buildSpanningTree=true)
 
void UpdateConnections (std::map< WAIKeyFrame *, int > KFcounter, bool buildSpanningTree)
 
void UpdateBestCovisibles ()
 
std::set< WAIKeyFrame * > GetConnectedKeyFrames ()
 
std::vector< WAIKeyFrame * > GetVectorCovisibleKeyFrames ()
 
std::vector< WAIKeyFrame * > GetBestCovisibilityKeyFrames (const int &N)
 
std::vector< WAIKeyFrame * > GetCovisiblesByWeight (const int &w)
 
int GetWeight (WAIKeyFrame *pKF)
 
const std::map< WAIKeyFrame *, int > & GetConnectedKfWeights ()
 
void AddChild (WAIKeyFrame *pKF)
 
void EraseChild (WAIKeyFrame *pKF)
 
void ChangeParent (WAIKeyFrame *pKF)
 
std::set< WAIKeyFrame * > GetChilds ()
 
WAIKeyFrameGetParent ()
 
bool hasChild (WAIKeyFrame *pKF)
 
void AddLoopEdge (WAIKeyFrame *pKF)
 
std::set< WAIKeyFrame * > GetLoopEdges ()
 
void AddMapPoint (WAIMapPoint *pMP, size_t idx)
 
void EraseMapPointMatch (WAIMapPoint *pMP)
 
void EraseMapPointMatch (const size_t &idx)
 
void ReplaceMapPointMatch (const size_t &idx, WAIMapPoint *pMP)
 
std::set< WAIMapPoint * > GetMapPoints ()
 
std::vector< WAIMapPoint * > GetMapPointMatches ()
 
int TrackedMapPoints (const int &minObs)
 
WAIMapPointGetMapPoint (const size_t &idx)
 
bool hasMapPoint (WAIMapPoint *mp)
 
bool isFixed () const
 
std::vector< size_t > GetFeaturesInArea (const float &x, const float &y, const float &r) const
 
bool IsInImage (const float &x, const float &y) const
 
void SetNotErase ()
 
void SetErase ()
 
void SetBadFlag ()
 
bool isBad ()
 
bool findChildRecursive (WAIKeyFrame *kf)
 
float ComputeSceneMedianDepth (const int q)
 
size_t getSizeOfCvMat (const cv::Mat &mat)
 
size_t getSizeOf ()
 
void setTexturePath (const std::string &path)
 
const std::string & getTexturePath ()
 
cv::Mat getObjectMatrix ()
 get visual representation as SLPoints More...
 

Static Public Member Functions

static bool weightComp (int a, int b)
 
static bool lId (WAIKeyFrame *pKF1, WAIKeyFrame *pKF2)
 

Public Attributes

long unsigned int mnId
 
const long unsigned int mnFrameId
 
const double mTimeStamp
 
const bool _fixed = false
 
const int mnGridCols
 
const int mnGridRows
 
const float mfGridElementWidthInv
 
const float mfGridElementHeightInv
 
long unsigned int mnMarker [7]
 
long unsigned int mnLoopQuery = 0
 
int mnLoopWords = 0
 
float mLoopScore = -1.0
 
long unsigned int mnRelocQuery = 0
 
int mnRelocWords = 0
 
float mRelocScore = -1.0f
 
cv::Mat mTcwGBA
 
cv::Mat mTcwRefGBA
 
const float fx
 
const float fy
 
const float cx
 
const float cy
 
const float invfx
 
const float invfy
 
const int N = 0
 
const std::vector< cv::KeyPoint > mvKeysUn
 
const cv::Mat mDescriptors
 
WAIBowVector mBowVec
 
WAIFeatVector mFeatVec
 
cv::Mat mTcp
 
const int mnScaleLevels
 
const float mfScaleFactor
 
const float mfLogScaleFactor
 
const std::vector< float > mvScaleFactors
 
const std::vector< float > mvLevelSigma2
 
const std::vector< float > mvInvLevelSigma2
 
const int mnMinX
 
const int mnMinY
 
const int mnMaxX
 
const int mnMaxY
 
const cv::Mat mK
 
cv::Mat imgGray
 
std::mutex mMutexPose
 
std::mutex mMutexConnections
 
std::mutex mMutexFeatures
 

Static Public Attributes

static long unsigned int nNextId = 0
 

Protected Attributes

cv::Mat _Twc
 
cv::Mat _Tcw
 
cv::Mat Ow
 camera center More...
 
std::vector< WAIMapPoint * > mvpMapPoints
 
std::vector< std::size_t > mGrid [FRAME_GRID_COLS][FRAME_GRID_ROWS]
 
std::map< WAIKeyFrame *, int > mConnectedKeyFrameWeights
 
std::vector< WAIKeyFrame * > mvpOrderedConnectedKeyFrames
 
std::vector< int > mvOrderedWeights
 
bool mbFirstConnection = true
 
WAIKeyFramempParent = NULL
 
std::set< WAIKeyFrame * > mspChildrens
 
std::set< WAIKeyFrame * > mspLoopEdges
 
bool mbNotErase
 
bool mbToBeErased
 
bool mbBad
 

Private Member Functions

void AssignFeaturesToGrid ()
 this is a function from Frame, but we need it here for map loading More...
 
bool PosInGrid (const cv::KeyPoint &kp, int &posX, int &posY)
 this is a function from Frame, but we need it here for map loading More...
 

Private Attributes

std::string _pathToTexture
 

Detailed Description

AR Keyframe node class.

A Keyframe is a camera with a position and additional information about key- points that were found in this frame. It also contains descriptors for the found keypoints.

Definition at line 59 of file WAIKeyFrame.h.

Constructor & Destructor Documentation

◆ WAIKeyFrame() [1/2]

WAIKeyFrame::WAIKeyFrame ( const cv::Mat &  Tcw,
unsigned long  id,
bool  fixKF,
float  fx,
float  fy,
float  cx,
float  cy,
size_t  N,
const std::vector< cv::KeyPoint > &  vKeysUn,
const cv::Mat &  descriptors,
WAIOrbVocabulary vocabulary,
int  nScaleLevels,
float  fScaleFactor,
const std::vector< float > &  vScaleFactors,
const std::vector< float > &  vLevelSigma2,
const std::vector< float > &  vInvLevelSigma2,
int  nMinX,
int  nMinY,
int  nMaxX,
int  nMaxY,
const cv::Mat &  KB 
)

keyframe generation during map loading

load an existing keyframe (used during file load)

Definition at line 40 of file WAIKeyFrame.cpp.

61  : mnId(id),
62  mnFrameId(0),
63  mTimeStamp(0),
66  mfGridElementWidthInv(static_cast<float>(FRAME_GRID_COLS) / (nMaxX - nMinX)),
67  mfGridElementHeightInv(static_cast<float>(FRAME_GRID_ROWS) / (nMaxY - nMinY)),
68  _fixed(fixKF),
69  mnLoopQuery(0),
70  mnLoopWords(0),
71  mnRelocQuery(0),
72  mnRelocWords(0),
73  fx(fx),
74  fy(fy),
75  cx(cx),
76  cy(cy),
77  invfx(1 / fx),
78  invfy(1 / fy),
79  N((int)N),
80  mvKeysUn(vKeysUn),
81  mDescriptors(descriptors.clone()),
82  mnScaleLevels(nScaleLevels),
83  mfScaleFactor(fScaleFactor),
84  mfLogScaleFactor(log(fScaleFactor)),
85  mvScaleFactors(vScaleFactors),
86  mvLevelSigma2(vLevelSigma2),
87  mvInvLevelSigma2(vInvLevelSigma2),
88  mnMinX(nMinX),
89  mnMinY(nMinY),
90  mnMaxX(nMaxX),
91  mnMaxY(nMaxY),
92  mK(K.clone()),
93  mbFirstConnection(true),
94  mpParent(NULL),
95  mbNotErase(false),
96  mbToBeErased(false),
97  mbBad(false)
98 {
99  mnMarker[0] = 0;
100  mnMarker[1] = 0;
101  mnMarker[2] = 0;
102  mnMarker[3] = 0;
103  mnMarker[4] = 0;
104  mnMarker[5] = 0;
105  mnMarker[6] = 0;
106  //Update next id so we never have twice the same id and especially only one with 0 (this is important)
107  if (id >= nNextId)
108  nNextId = id + 1;
109 
110  mvpMapPoints = vector<WAIMapPoint*>(N, static_cast<WAIMapPoint*>(NULL));
111  //set camera position
112  SetPose(Tcw);
113 
114  //compute mBowVec and mFeatVec
115  ComputeBoW(vocabulary);
116 
117  //assign features to grid
119 }
#define FRAME_GRID_COLS
Definition: WAIFrame.h:44
#define FRAME_GRID_ROWS
Definition: WAIFrame.h:43
const float cy
Definition: WAIKeyFrame.h:214
const cv::Mat mK
Definition: WAIKeyFrame.h:245
bool mbFirstConnection
Definition: WAIKeyFrame.h:275
void SetPose(const cv::Mat &Tcw)
const int mnMaxX
Definition: WAIKeyFrame.h:243
const std::vector< float > mvInvLevelSigma2
Definition: WAIKeyFrame.h:238
const float mfGridElementHeightInv
Definition: WAIKeyFrame.h:187
const int mnScaleLevels
Definition: WAIKeyFrame.h:233
std::vector< WAIMapPoint * > mvpMapPoints
Definition: WAIKeyFrame.h:263
const int mnGridCols
Definition: WAIKeyFrame.h:184
const int mnMinY
Definition: WAIKeyFrame.h:242
const double mTimeStamp
Definition: WAIKeyFrame.h:178
bool mbNotErase
Definition: WAIKeyFrame.h:281
const std::vector< float > mvScaleFactors
Definition: WAIKeyFrame.h:236
int mnRelocWords
Definition: WAIKeyFrame.h:206
const long unsigned int mnFrameId
Definition: WAIKeyFrame.h:176
const float fx
Definition: WAIKeyFrame.h:214
long unsigned int mnId
Definition: WAIKeyFrame.h:175
const float mfScaleFactor
Definition: WAIKeyFrame.h:234
const int mnMaxY
Definition: WAIKeyFrame.h:244
WAIKeyFrame * mpParent
Definition: WAIKeyFrame.h:276
const int mnMinX
Definition: WAIKeyFrame.h:241
const int mnGridRows
Definition: WAIKeyFrame.h:185
const bool _fixed
Definition: WAIKeyFrame.h:181
const std::vector< cv::KeyPoint > mvKeysUn
Definition: WAIKeyFrame.h:220
const float mfGridElementWidthInv
Definition: WAIKeyFrame.h:186
long unsigned int mnRelocQuery
Definition: WAIKeyFrame.h:205
long unsigned int mnLoopQuery
Definition: WAIKeyFrame.h:202
const float fy
Definition: WAIKeyFrame.h:214
const float cx
Definition: WAIKeyFrame.h:214
const float invfy
Definition: WAIKeyFrame.h:214
bool mbToBeErased
Definition: WAIKeyFrame.h:282
void AssignFeaturesToGrid()
this is a function from Frame, but we need it here for map loading
const float invfx
Definition: WAIKeyFrame.h:214
long unsigned int mnMarker[7]
Definition: WAIKeyFrame.h:199
const int N
Definition: WAIKeyFrame.h:217
const std::vector< float > mvLevelSigma2
Definition: WAIKeyFrame.h:237
void ComputeBoW(WAIOrbVocabulary *vocabulary)
const cv::Mat mDescriptors
Definition: WAIKeyFrame.h:223
const float mfLogScaleFactor
Definition: WAIKeyFrame.h:235
static long unsigned int nNextId
Definition: WAIKeyFrame.h:174
void log(const char *tag, const char *format,...)
logs a formatted string platform independently
Definition: Utils.cpp:1103

◆ WAIKeyFrame() [2/2]

WAIKeyFrame::WAIKeyFrame ( WAIFrame F,
bool  retainImg = true 
)

keyframe generation from frame

Definition at line 121 of file WAIKeyFrame.cpp.

122  : mnFrameId(F.mnId),
128  _fixed(false),
129  mnLoopQuery(0),
130  mnLoopWords(0),
131  mnRelocQuery(0),
132  mnRelocWords(0),
133  fx(F.fx),
134  fy(F.fy),
135  cx(F.cx),
136  cy(F.cy),
137  invfx(F.invfx),
138  invfy(F.invfy),
139  /* mbf(F.mbf), mb(F.mb), mThDepth(F.mThDepth),*/ N(F.N),
140  /*mvKeys(F.mvKeys),*/ mvKeysUn(F.mvKeysUn),
141  /* mvuRight(F.mvuRight), mvDepth(F.mvDepth),*/ mDescriptors(F.mDescriptors.clone()),
142  mBowVec(F.mBowVec),
143  mFeatVec(F.mFeatVec),
150  mnMinX((int)F.mnMinX),
151  mnMinY((int)F.mnMinY),
152  mnMaxX((int)F.mnMaxX),
153  mnMaxY((int)F.mnMaxY),
154  mK(F.mK),
156  /*mpORBvocabulary(F.mpORBvocabulary),*/ mbFirstConnection(true),
157  mpParent(NULL),
158  mbNotErase(false),
159  mbToBeErased(false),
160  mbBad(false) /*, mHalfBaseline(F.mb / 2)*/
161 {
162  mnMarker[0] = 0;
163  mnMarker[1] = 0;
164  mnMarker[2] = 0;
165  mnMarker[3] = 0;
166  mnMarker[4] = 0;
167  mnMarker[5] = 0;
168  mnMarker[6] = 0;
169  mnId = nNextId++;
170 
171  for (int i = 0; i < FRAME_GRID_COLS; i++)
172  for (int j = 0; j < FRAME_GRID_ROWS; j++)
173  mGrid[i][j] = F.mGrid[i][j];
174 
175  //mGrid.resize(mnGridCols);
176  //for (int i = 0; i<mnGridCols; i++)
177  //{
178  // mGrid[i].resize(mnGridRows);
179  // for (int j = 0; j<mnGridRows; j++)
180  // mGrid[i][j] = F.mGrid[i][j];
181  //}
182 
183  SetPose(F.mTcw);
184 
185  if (retainImg && !F.imgGray.empty())
186  imgGray = F.imgGray;
187 }
cv::Mat mTcw
Definition: WAIFrame.h:155
float mfScaleFactor
Definition: WAIFrame.h:169
static float cx
Definition: WAIFrame.h:116
WAIFeatVector mFeatVec
Definition: WAIFrame.h:138
static float fx
Definition: WAIFrame.h:114
cv::Mat mDescriptors
Definition: WAIFrame.h:141
int mnScaleLevels
Definition: WAIFrame.h:168
std::vector< float > mvScaleFactors
Definition: WAIFrame.h:171
static float mnMaxX
Definition: WAIFrame.h:178
static float fy
Definition: WAIFrame.h:115
static float cy
Definition: WAIFrame.h:117
std::vector< float > mvLevelSigma2
Definition: WAIFrame.h:173
static float mnMinY
Definition: WAIFrame.h:179
std::vector< WAIMapPoint * > mvpMapPoints
Definition: WAIFrame.h:147
long unsigned int mnId
Definition: WAIFrame.h:159
WAIBowVector mBowVec
Definition: WAIFrame.h:137
static float mfGridElementHeightInv
Definition: WAIFrame.h:151
cv::Mat imgGray
Definition: WAIFrame.h:185
static float invfx
Definition: WAIFrame.h:118
std::vector< std::size_t > mGrid[64][36]
Definition: WAIFrame.h:152
static float mnMaxY
Definition: WAIFrame.h:180
std::vector< float > mvInvLevelSigma2
Definition: WAIFrame.h:174
cv::Mat mK
Definition: WAIFrame.h:113
static float mnMinX
Definition: WAIFrame.h:177
int N
Definition: WAIFrame.h:123
static float mfGridElementWidthInv
Definition: WAIFrame.h:150
std::vector< cv::KeyPoint > mvKeysUn
Definition: WAIFrame.h:129
float mfLogScaleFactor
Definition: WAIFrame.h:170
double mTimeStamp
Definition: WAIFrame.h:110
static float invfy
Definition: WAIFrame.h:119
WAIBowVector mBowVec
Definition: WAIKeyFrame.h:226
std::vector< std::size_t > mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]
Definition: WAIKeyFrame.h:266
WAIFeatVector mFeatVec
Definition: WAIKeyFrame.h:227
cv::Mat imgGray
Definition: WAIKeyFrame.h:248

Member Function Documentation

◆ AddChild()

void WAIKeyFrame::AddChild ( WAIKeyFrame pKF)

Definition at line 538 of file WAIKeyFrame.cpp.

539 {
540  unique_lock<mutex> lockCon(mMutexConnections);
541  mspChildrens.insert(pKF);
542 }
std::mutex mMutexConnections
Definition: WAIKeyFrame.h:287
std::set< WAIKeyFrame * > mspChildrens
Definition: WAIKeyFrame.h:277

◆ AddConnection()

void WAIKeyFrame::AddConnection ( WAIKeyFrame pKF,
int  weight 
)

Definition at line 265 of file WAIKeyFrame.cpp.

266 {
267  {
268  unique_lock<mutex> lock(mMutexConnections);
269  if (!mConnectedKeyFrameWeights.count(pKF))
270  mConnectedKeyFrameWeights[pKF] = weight;
271  else if (mConnectedKeyFrameWeights[pKF] != weight)
272  mConnectedKeyFrameWeights[pKF] = weight;
273  else
274  return;
275  }
276 
278 }
std::map< WAIKeyFrame *, int > mConnectedKeyFrameWeights
Definition: WAIKeyFrame.h:269
void UpdateBestCovisibles()

◆ AddLoopEdge()

void WAIKeyFrame::AddLoopEdge ( WAIKeyFrame pKF)

Definition at line 575 of file WAIKeyFrame.cpp.

576 {
577  unique_lock<mutex> lockCon(mMutexConnections);
578  mbNotErase = true;
579  mspLoopEdges.insert(pKF);
580 }
std::set< WAIKeyFrame * > mspLoopEdges
Definition: WAIKeyFrame.h:278

◆ AddMapPoint()

void WAIKeyFrame::AddMapPoint ( WAIMapPoint pMP,
size_t  idx 
)

Definition at line 357 of file WAIKeyFrame.cpp.

358 {
359  unique_lock<mutex> lock(mMutexFeatures);
360 
361  mvpMapPoints[idx] = pMP;
362 }
std::mutex mMutexFeatures
Definition: WAIKeyFrame.h:288

◆ AssignFeaturesToGrid()

void WAIKeyFrame::AssignFeaturesToGrid ( )
private

this is a function from Frame, but we need it here for map loading

Definition at line 857 of file WAIKeyFrame.cpp.

858 {
859  PROFILE_SCOPE("WAI::WAIKeyFrame::AssignFeaturesToGrid");
860 
861  int nReserve = (int)(0.5f * N / (FRAME_GRID_COLS * FRAME_GRID_ROWS));
862  for (unsigned int i = 0; i < FRAME_GRID_COLS; i++)
863  for (unsigned int j = 0; j < FRAME_GRID_ROWS; j++)
864  mGrid[i][j].reserve(nReserve);
865 
866  for (int i = 0; i < N; i++)
867  {
868  const cv::KeyPoint& kp = mvKeysUn[i];
869 
870  int nGridPosX, nGridPosY;
871  if (PosInGrid(kp, nGridPosX, nGridPosY))
872  mGrid[nGridPosX][nGridPosY].push_back(i);
873  }
874 }
#define PROFILE_SCOPE(name)
Definition: Instrumentor.h:40
bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY)
this is a function from Frame, but we need it here for map loading

◆ ChangeParent()

void WAIKeyFrame::ChangeParent ( WAIKeyFrame pKF)

Definition at line 550 of file WAIKeyFrame.cpp.

551 {
552  unique_lock<mutex> lockCon(mMutexConnections);
553  mpParent = pKF;
554  pKF->AddChild(this);
555 }
void AddChild(WAIKeyFrame *pKF)

◆ ComputeBoW()

void WAIKeyFrame::ComputeBoW ( WAIOrbVocabulary vocabulary)

Definition at line 196 of file WAIKeyFrame.cpp.

197 {
198  PROFILE_SCOPE("WAI::WAIKeyFrame::ComputeBoW");
199 
200  if (mBowVec.data.empty() || mFeatVec.data.empty())
201  {
202  //vector<cv::Mat> vCurrentDesc = ORB_SLAM2::Converter::toDescriptorVector(mDescriptors);
203  // Feature vector associate features with nodes in the 4th level (from leaves up)
204  // We assume the vocabulary tree has 6 levels, change the 4 otherwise
205 
206  // Luc: In a 6 levels and 10 branch per level voc, 4 levelup mean the 2nd level from the top
207  // that make a total of 100 words. More words means more variance between keyframe and less
208  // preselected keyframe but that will make also the relocalization less invariant to changes
209  //vCurrentDesc, mBowVec, mFeatVec, orbVocabulary->getDepthLevels() - 2
210 
211  //TODO ensure level is now from the top
212  vocabulary->transform(mDescriptors, mBowVec, mFeatVec);
213  }
214 }
void transform(const cv::Mat &descriptors, WAIBowVector &bow, WAIFeatVector &feat)
fbow::fBow data
fbow::fBow2 data

◆ ComputeSceneMedianDepth()

float WAIKeyFrame::ComputeSceneMedianDepth ( const int  q)

Definition at line 817 of file WAIKeyFrame.cpp.

818 {
819  vector<WAIMapPoint*> vpMapPoints;
820  cv::Mat Tcw_;
821  {
822  unique_lock<mutex> lock(mMutexFeatures);
823  unique_lock<mutex> lock2(mMutexPose);
824  vpMapPoints = mvpMapPoints;
825  Tcw_ = _Tcw.clone();
826  }
827 
828  vector<float> vDepths;
829  vDepths.reserve(N);
830  cv::Mat Rcw2 = Tcw_.row(2).colRange(0, 3);
831  Rcw2 = Rcw2.t();
832  float zcw = Tcw_.at<float>(2, 3);
833  for (int i = 0; i < N; i++)
834  {
835  if (mvpMapPoints[i])
836  {
837  WAIMapPoint* pMP = mvpMapPoints[i];
838  cv::Mat x3Dw = pMP->GetWorldPos();
839  float z = (float)Rcw2.dot(x3Dw) + zcw;
840  vDepths.push_back(z);
841  }
842  }
843 
844  sort(vDepths.begin(), vDepths.end());
845 
846  return vDepths[(vDepths.size() - 1) / q];
847 }
std::mutex mMutexPose
Definition: WAIKeyFrame.h:286
cv::Mat _Tcw
Definition: WAIKeyFrame.h:257
cv::Mat GetWorldPos()

◆ EraseChild()

void WAIKeyFrame::EraseChild ( WAIKeyFrame pKF)

Definition at line 544 of file WAIKeyFrame.cpp.

545 {
546  unique_lock<mutex> lockCon(mMutexConnections);
547  mspChildrens.erase(pKF);
548 }

◆ EraseConnection()

void WAIKeyFrame::EraseConnection ( WAIKeyFrame pKF)

Definition at line 754 of file WAIKeyFrame.cpp.

755 {
756  bool bUpdate = false;
757  {
758  unique_lock<mutex> lock(mMutexConnections);
759  if (mConnectedKeyFrameWeights.count(pKF))
760  {
761  mConnectedKeyFrameWeights.erase(pKF);
762  bUpdate = true;
763  }
764  }
765 
766  if (bUpdate)
768 }

◆ EraseMapPointMatch() [1/2]

void WAIKeyFrame::EraseMapPointMatch ( const size_t &  idx)

Definition at line 364 of file WAIKeyFrame.cpp.

365 {
366  unique_lock<mutex> lock(mMutexFeatures);
367  mvpMapPoints[idx] = static_cast<WAIMapPoint*>(NULL);
368 }

◆ EraseMapPointMatch() [2/2]

void WAIKeyFrame::EraseMapPointMatch ( WAIMapPoint pMP)

Definition at line 370 of file WAIKeyFrame.cpp.

371 {
372  unique_lock<mutex> lock(mMutexFeatures);
373  int idx = pMP->GetIndexInKeyFrame(this);
374  if (idx >= 0)
375  mvpMapPoints[idx] = static_cast<WAIMapPoint*>(NULL);
376 }
int GetIndexInKeyFrame(WAIKeyFrame *pKF)

◆ FindAndUpdateConnections()

void WAIKeyFrame::FindAndUpdateConnections ( bool  buildSpanningTree = true)

Definition at line 437 of file WAIKeyFrame.cpp.

438 {
439  //ghm1: a covisibility graph between keyframes (nodes) is maintained:
440  //if two keyframes share more than 15 observations of the same map points an edge is added. The number of the common observations is the edge weight.
441  map<WAIKeyFrame*, int> KFcounter;
442 
443  vector<WAIMapPoint*> vpMP;
444 
445  {
446  unique_lock<mutex> lockMPs(mMutexFeatures);
447  vpMP = mvpMapPoints;
448  }
449 
450  //For all map points in keyframe check in which other keyframes are they seen
451  //Increase counter for those keyframes
452  for (vector<WAIMapPoint*>::iterator vit = vpMP.begin(), vend = vpMP.end(); vit != vend; vit++)
453  {
454  WAIMapPoint* pMP = *vit;
455 
456  if (!pMP)
457  continue;
458 
459  if (pMP->isBad())
460  continue;
461 
462  map<WAIKeyFrame*, size_t> observations = pMP->GetObservations();
463 
464  for (map<WAIKeyFrame*, size_t>::iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++)
465  {
466  if (mit->first->mnId == mnId)
467  continue;
468  KFcounter[mit->first]++;
469  }
470  }
471 
472  UpdateConnections(KFcounter, buildSpanningTree);
473 }
void UpdateConnections(std::map< WAIKeyFrame *, int > KFcounter, bool buildSpanningTree)
std::map< WAIKeyFrame *, size_t > GetObservations()

◆ findChildRecursive()

bool WAIKeyFrame::findChildRecursive ( WAIKeyFrame kf)

Definition at line 730 of file WAIKeyFrame.cpp.

731 {
732  for (auto it = mspChildrens.begin(); it != mspChildrens.end(); ++it)
733  {
734  if (*it != kf)
735  {
736  return (*it)->findChildRecursive(kf);
737  }
738  else
739  {
740  std::cout << "findChildRecursive found among children of id: " << (*it)->mnId << std::endl;
741  return true;
742  }
743  }
744 
745  return false;
746 }

◆ GetBestCovisibilityKeyFrames()

vector< WAIKeyFrame * > WAIKeyFrame::GetBestCovisibilityKeyFrames ( const int &  N)

Definition at line 316 of file WAIKeyFrame.cpp.

317 {
318  unique_lock<mutex> lock(mMutexConnections);
319  if ((int)mvpOrderedConnectedKeyFrames.size() < N)
321  else
322  return vector<WAIKeyFrame*>(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.begin() + N);
323 }
std::vector< WAIKeyFrame * > mvpOrderedConnectedKeyFrames
Definition: WAIKeyFrame.h:271

◆ GetCameraCenter()

cv::Mat WAIKeyFrame::GetCameraCenter ( )

Definition at line 247 of file WAIKeyFrame.cpp.

248 {
249  unique_lock<mutex> lock(mMutexPose);
250  return Ow.clone();
251 }
cv::Mat Ow
camera center
Definition: WAIKeyFrame.h:259

◆ GetChilds()

std::set< WAIKeyFrame * > WAIKeyFrame::GetChilds ( )

Definition at line 557 of file WAIKeyFrame.cpp.

558 {
559  unique_lock<mutex> lockCon(mMutexConnections);
560  return mspChildrens;
561 }

◆ GetConnectedKeyFrames()

set< WAIKeyFrame * > WAIKeyFrame::GetConnectedKeyFrames ( )

Definition at line 301 of file WAIKeyFrame.cpp.

302 {
303  unique_lock<mutex> lock(mMutexConnections);
304  set<WAIKeyFrame*> s;
305  for (map<WAIKeyFrame*, int>::iterator mit = mConnectedKeyFrameWeights.begin(); mit != mConnectedKeyFrameWeights.end(); mit++)
306  s.insert(mit->first);
307  return s;
308 }
SLScene * s
Definition: SLScene.h:31
The SLScene class represents the top level instance holding the scene structure.
Definition: SLScene.h:47

◆ GetConnectedKfWeights()

const std::map< WAIKeyFrame *, int > & WAIKeyFrame::GetConnectedKfWeights ( )

Definition at line 351 of file WAIKeyFrame.cpp.

352 {
353  unique_lock<mutex> lock(mMutexConnections);
355 }

◆ GetCovisiblesByWeight()

vector< WAIKeyFrame * > WAIKeyFrame::GetCovisiblesByWeight ( const int &  w)

Definition at line 325 of file WAIKeyFrame.cpp.

326 {
327  unique_lock<mutex> lock(mMutexConnections);
328 
329  if (mvpOrderedConnectedKeyFrames.empty())
330  return vector<WAIKeyFrame*>();
331 
332  vector<int>::iterator it = upper_bound(mvOrderedWeights.begin(), mvOrderedWeights.end(), w, WAIKeyFrame::weightComp);
333  if (it == mvOrderedWeights.end())
334  return vector<WAIKeyFrame*>();
335  else
336  {
337  int n = (int)(it - mvOrderedWeights.begin());
338  return vector<WAIKeyFrame*>(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.begin() + n);
339  }
340 }
std::vector< int > mvOrderedWeights
Definition: WAIKeyFrame.h:272
static bool weightComp(int a, int b)
Definition: WAIKeyFrame.h:158

◆ GetFeaturesInArea()

vector< size_t > WAIKeyFrame::GetFeaturesInArea ( const float &  x,
const float &  y,
const float &  r 
) const

Definition at line 770 of file WAIKeyFrame.cpp.

771 {
772  vector<size_t> vIndices;
773  vIndices.reserve(N);
774 
775  const int nMinCellX = max(0, (int)floor((x - mnMinX - r) * mfGridElementWidthInv));
776  if (nMinCellX >= mnGridCols)
777  return vIndices;
778 
779  const int nMaxCellX = min(mnGridCols - 1, (int)ceil((x - mnMinX + r) * mfGridElementWidthInv));
780  if (nMaxCellX < 0)
781  return vIndices;
782 
783  const int nMinCellY = max(0, (int)floor((y - mnMinY - r) * mfGridElementHeightInv));
784  if (nMinCellY >= mnGridRows)
785  return vIndices;
786 
787  const int nMaxCellY = min(mnGridRows - 1, (int)ceil((y - mnMinY + r) * mfGridElementHeightInv));
788  if (nMaxCellY < 0)
789  return vIndices;
790 
791  for (int ix = nMinCellX; ix <= nMaxCellX; ix++)
792  {
793  for (int iy = nMinCellY; iy <= nMaxCellY; iy++)
794  {
795  const vector<size_t> vCell = mGrid[ix][iy];
796  for (size_t j = 0, jend = vCell.size(); j < jend; j++)
797  {
798  const cv::KeyPoint& kpUn = mvKeysUn[vCell[j]];
799  const float distx = kpUn.pt.x - x;
800  const float disty = kpUn.pt.y - y;
801 
802  if (fabs(distx) < r && fabs(disty) < r)
803  vIndices.push_back(vCell[j]);
804  }
805  }
806  }
807 
808  return vIndices;
809 }
T ceil(T a)
Definition: Utils.h:247
T floor(T a)
Definition: Utils.h:246

◆ GetLoopEdges()

set< WAIKeyFrame * > WAIKeyFrame::GetLoopEdges ( )

Definition at line 582 of file WAIKeyFrame.cpp.

583 {
584  unique_lock<mutex> lockCon(mMutexConnections);
585  return mspLoopEdges;
586 }

◆ GetMapPoint()

WAIMapPoint * WAIKeyFrame::GetMapPoint ( const size_t &  idx)

Definition at line 431 of file WAIKeyFrame.cpp.

432 {
433  unique_lock<mutex> lock(mMutexFeatures);
434  return mvpMapPoints[idx];
435 }

◆ GetMapPointMatches()

vector< WAIMapPoint * > WAIKeyFrame::GetMapPointMatches ( )

Definition at line 425 of file WAIKeyFrame.cpp.

426 {
427  unique_lock<mutex> lock(mMutexFeatures);
428  return mvpMapPoints;
429 }

◆ GetMapPoints()

set< WAIMapPoint * > WAIKeyFrame::GetMapPoints ( )

Definition at line 383 of file WAIKeyFrame.cpp.

384 {
385  unique_lock<mutex> lock(mMutexFeatures);
386  set<WAIMapPoint*> s;
387  for (size_t i = 0, iend = mvpMapPoints.size(); i < iend; i++)
388  {
389  if (!mvpMapPoints[i])
390  continue;
391  WAIMapPoint* pMP = mvpMapPoints[i];
392  if (!pMP->isBad())
393  s.insert(pMP);
394  }
395  return s;
396 }

◆ getObjectMatrix()

cv::Mat WAIKeyFrame::getObjectMatrix ( )

get visual representation as SLPoints

Definition at line 849 of file WAIKeyFrame.cpp.

850 {
851  cv::Mat result = _Twc.clone();
852 
853  return result;
854 }
cv::Mat _Twc
Definition: WAIKeyFrame.h:256

◆ GetParent()

WAIKeyFrame * WAIKeyFrame::GetParent ( )

Definition at line 563 of file WAIKeyFrame.cpp.

564 {
565  unique_lock<mutex> lockCon(mMutexConnections);
566  return mpParent;
567 }

◆ GetPose()

cv::Mat WAIKeyFrame::GetPose ( )

Definition at line 235 of file WAIKeyFrame.cpp.

236 {
237  unique_lock<mutex> lock(mMutexPose);
238  return _Tcw.clone();
239 }

◆ GetPoseInverse()

cv::Mat WAIKeyFrame::GetPoseInverse ( )

Definition at line 241 of file WAIKeyFrame.cpp.

242 {
243  unique_lock<mutex> lock(mMutexPose);
244  return _Twc.clone();
245 }

◆ GetRotation()

cv::Mat WAIKeyFrame::GetRotation ( )

Definition at line 253 of file WAIKeyFrame.cpp.

254 {
255  unique_lock<mutex> lock(mMutexPose);
256  return _Tcw.rowRange(0, 3).colRange(0, 3).clone();
257 }

◆ getSizeOf()

size_t WAIKeyFrame::getSizeOf ( )

Definition at line 902 of file WAIKeyFrame.cpp.

903 {
904  size_t size = 0;
905 
906  size += sizeof(*this);
907 
908  //size_t testImg = sizeof(imgGray);
909  //size_t testImg2 = getSizeOfCvMat(imgGray);
910 
911  //size_t test1 = sizeof(mDescriptors);
912  //size_t test2 = getSizeOfCvMat(mDescriptors);
913  //add space for cv mats:
914  size += getSizeOfCvMat(mTcwGBA);
915  size += getSizeOfCvMat(mTcwRefGBA);
916  size += getSizeOfCvMat(mDescriptors);
917  size += getSizeOfCvMat(mTcp);
918  size += getSizeOfCvMat(imgGray);
919  size += getSizeOfCvMat(_Twc);
920  size += getSizeOfCvMat(Ow);
921 
922  return size;
923 }
cv::Mat mTcwRefGBA
Definition: WAIKeyFrame.h:211
cv::Mat mTcp
Definition: WAIKeyFrame.h:230
size_t getSizeOfCvMat(const cv::Mat &mat)
cv::Mat mTcwGBA
Definition: WAIKeyFrame.h:210

◆ getSizeOfCvMat()

size_t WAIKeyFrame::getSizeOfCvMat ( const cv::Mat &  mat)

Definition at line 889 of file WAIKeyFrame.cpp.

890 {
891  size_t size = 0;
892  if (mat.isContinuous())
893  size = mat.total() * mat.elemSize();
894  else
895  {
896  size = mat.step[0] * mat.rows;
897  }
898  return size;
899 }

◆ getTexturePath()

const std::string& WAIKeyFrame::getTexturePath ( )
inline

Definition at line 292 of file WAIKeyFrame.h.

292 { return _pathToTexture; }
std::string _pathToTexture
Definition: WAIKeyFrame.h:308

◆ GetTranslation()

cv::Mat WAIKeyFrame::GetTranslation ( )

Definition at line 259 of file WAIKeyFrame.cpp.

260 {
261  unique_lock<mutex> lock(mMutexPose);
262  return _Tcw.rowRange(0, 3).col(3).clone();
263 }

◆ GetVectorCovisibleKeyFrames()

vector< WAIKeyFrame * > WAIKeyFrame::GetVectorCovisibleKeyFrames ( )

Definition at line 310 of file WAIKeyFrame.cpp.

311 {
312  unique_lock<mutex> lock(mMutexConnections);
314 }

◆ GetWeight()

int WAIKeyFrame::GetWeight ( WAIKeyFrame pKF)

Definition at line 342 of file WAIKeyFrame.cpp.

343 {
344  unique_lock<mutex> lock(mMutexConnections);
345  if (mConnectedKeyFrameWeights.count(pKF))
346  return mConnectedKeyFrameWeights[pKF];
347  else
348  return 0;
349 }

◆ hasChild()

bool WAIKeyFrame::hasChild ( WAIKeyFrame pKF)

Definition at line 569 of file WAIKeyFrame.cpp.

570 {
571  unique_lock<mutex> lockCon(mMutexConnections);
572  return mspChildrens.count(pKF);
573 }

◆ hasMapPoint()

bool WAIKeyFrame::hasMapPoint ( WAIMapPoint mp)

Definition at line 925 of file WAIKeyFrame.cpp.

926 {
927  bool result = false;
928 
929  for (WAIMapPoint* mmp : mvpMapPoints)
930  {
931  if (mmp == mp)
932  {
933  result = true;
934  break;
935  }
936  }
937 
938  return result;
939 }

◆ isBad()

bool WAIKeyFrame::isBad ( )

Definition at line 748 of file WAIKeyFrame.cpp.

749 {
750  unique_lock<mutex> lock(mMutexConnections);
751  return mbBad;
752 }

◆ isFixed()

bool WAIKeyFrame::isFixed ( ) const

Definition at line 941 of file WAIKeyFrame.cpp.

942 {
943  return _fixed;
944 }

◆ IsInImage()

bool WAIKeyFrame::IsInImage ( const float &  x,
const float &  y 
) const

Definition at line 811 of file WAIKeyFrame.cpp.

812 {
813  return (x >= mnMinX && x < mnMaxX && y >= mnMinY && y < mnMaxY);
814 }

◆ lId()

static bool WAIKeyFrame::lId ( WAIKeyFrame pKF1,
WAIKeyFrame pKF2 
)
inlinestatic

Definition at line 163 of file WAIKeyFrame.h.

164  {
165  return pKF1->mnId < pKF2->mnId;
166  }

◆ PosInGrid()

bool WAIKeyFrame::PosInGrid ( const cv::KeyPoint &  kp,
int &  posX,
int &  posY 
)
private

this is a function from Frame, but we need it here for map loading

Definition at line 877 of file WAIKeyFrame.cpp.

878 {
879  posX = (int)round((kp.pt.x - mnMinX) * mfGridElementWidthInv);
880  posY = (int)round((kp.pt.y - mnMinY) * mfGridElementHeightInv);
881 
882  //Keypoint's coordinates are undistorted, which could cause to go out of the image
883  if (posX < 0 || posX >= FRAME_GRID_COLS || posY < 0 || posY >= FRAME_GRID_ROWS)
884  return false;
885 
886  return true;
887 }

◆ ReplaceMapPointMatch()

void WAIKeyFrame::ReplaceMapPointMatch ( const size_t &  idx,
WAIMapPoint pMP 
)

Definition at line 378 of file WAIKeyFrame.cpp.

379 {
380  mvpMapPoints[idx] = pMP;
381 }

◆ SetBadFlag()

void WAIKeyFrame::SetBadFlag ( )

Definition at line 610 of file WAIKeyFrame.cpp.

611 {
612  {
613  unique_lock<mutex> lock(mMutexConnections);
614  if (mnId == 0)
615  {
616  //never delete first keyframe
617  return;
618  }
619  else if (mbNotErase)
620  {
621  //never delete keyframes with this flag
622  mbToBeErased = true;
623  return;
624  }
625  }
626 
627  //for all connected keyframes remove this as a neighbour
628  for (map<WAIKeyFrame*, int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend = mConnectedKeyFrameWeights.end(); mit != mend; mit++)
629  {
630  mit->first->EraseConnection(this);
631  }
632 
633  //erase observation from keypoints observed by this keyframe
634  for (size_t i = 0; i < mvpMapPoints.size(); i++)
635  {
636  if (mvpMapPoints[i])
637  {
638  mvpMapPoints[i]->EraseObservation(this);
639  }
640  }
641 
642  {
643  unique_lock<mutex> lock(mMutexConnections);
644  unique_lock<mutex> lock1(mMutexFeatures);
645 
648 
649  // ghm1: Update Spanning Tree: As we try to cull a keyframe we have to update the parent keyframe from all his children.
650  // A parent keyframe is the one that has the best covisibility with his potential child. So it is not necessaryly the parent of the culled keyframe.
651  // Rather we search all connected keyframes in covisibilty graph (see GetWeight) for every child to find the best parent
652  set<WAIKeyFrame*> sParentCandidates;
653  sParentCandidates.insert(mpParent);
654 
655  // Assign at each iteration one child with a parent (the pair with highest covisibility weight)
656  // Include that child as new parent candidate for the rest
657  while (!mspChildrens.empty())
658  {
659  bool bContinue = false;
660 
661  int max = -1;
662  WAIKeyFrame* pC;
663  WAIKeyFrame* pP;
664 
665  for (set<WAIKeyFrame*>::iterator sit = mspChildrens.begin(), send = mspChildrens.end(); sit != send;)
666  {
667  WAIKeyFrame* pKF = *sit;
668  if (pKF->isBad())
669  {
670  sit = mspChildrens.erase(sit);
671  continue;
672  }
673 
674  // Check if a parent candidate is connected to the keyframe
675  vector<WAIKeyFrame*> vpConnected = pKF->GetVectorCovisibleKeyFrames();
676  for (size_t i = 0, iend = vpConnected.size(); i < iend; i++)
677  {
678  if (vpConnected[i]->mnId == this->mnId || vpConnected[i]->isBad()) { continue; }
679 
680  for (set<WAIKeyFrame*>::iterator spcit = sParentCandidates.begin(), spcend = sParentCandidates.end(); spcit != spcend; spcit++)
681  {
682  if (vpConnected[i]->mnId == (*spcit)->mnId)
683  {
684  int w = pKF->GetWeight(vpConnected[i]);
685  if (w > max)
686  {
687  pC = pKF;
688  pP = vpConnected[i];
689  max = w;
690  bContinue = true;
691  }
692  }
693  }
694  }
695 
696  sit++;
697  }
698 
699  if (bContinue)
700  {
701  pC->ChangeParent(pP);
702  sParentCandidates.insert(pC);
703  mspChildrens.erase(pC);
704  }
705  else
706  {
707  break;
708  }
709  }
710 
711  // If a children has no covisibility links with any parent candidate, assign to the original parent of this KF
712  //ghm1: (change because of exception in ChangeParent) check that the parent is not the child itself. If this is
713  //the case, remove itself from covisibles and use the second best covisible as parent.
714  if (!mspChildrens.empty())
715  {
716  for (set<WAIKeyFrame*>::iterator sit = mspChildrens.begin(); sit != mspChildrens.end(); sit++)
717  {
718  (*sit)->ChangeParent(mpParent);
719  }
720  }
721 
722  mpParent->EraseChild(this);
724  mbBad = true;
725  }
726 
727  //_kfDb->erase(this);
728 }
AR Keyframe node class.
Definition: WAIKeyFrame.h:60
void ChangeParent(WAIKeyFrame *pKF)
void EraseChild(WAIKeyFrame *pKF)
std::vector< WAIKeyFrame * > GetVectorCovisibleKeyFrames()
cv::Mat GetPoseInverse()
int GetWeight(WAIKeyFrame *pKF)

◆ SetBowVector()

void WAIKeyFrame::SetBowVector ( WAIBowVector bow)

Definition at line 190 of file WAIKeyFrame.cpp.

191 {
192  mBowVec = bow;
193 }

◆ SetErase()

void WAIKeyFrame::SetErase ( )

Definition at line 594 of file WAIKeyFrame.cpp.

595 {
596  {
597  unique_lock<mutex> lock(mMutexConnections);
598  if (mspLoopEdges.empty())
599  {
600  mbNotErase = false;
601  }
602  }
603 
604  if (mbToBeErased)
605  {
606  SetBadFlag();
607  }
608 }
void SetBadFlag()

◆ SetNotErase()

void WAIKeyFrame::SetNotErase ( )

Definition at line 588 of file WAIKeyFrame.cpp.

589 {
590  unique_lock<mutex> lock(mMutexConnections);
591  mbNotErase = true;
592 }

◆ SetPose()

void WAIKeyFrame::SetPose ( const cv::Mat &  Tcw)

Definition at line 216 of file WAIKeyFrame.cpp.

217 {
218  PROFILE_SCOPE("WAI::WAIKeyFrame::SetPose");
219 
220  unique_lock<mutex> lock(mMutexPose);
221  Tcw.copyTo(_Tcw);
222  cv::Mat Rcw = _Tcw.rowRange(0, 3).colRange(0, 3);
223  cv::Mat tcw = _Tcw.rowRange(0, 3).col(3);
224  cv::Mat Rwc = Rcw.t();
225  Ow = -Rwc * tcw;
226 
227  _Twc = cv::Mat::eye(4, 4, Tcw.type());
228  Rwc.copyTo(_Twc.rowRange(0, 3).colRange(0, 3));
229  Ow.copyTo(_Twc.rowRange(0, 3).col(3));
230  //ghm1: unused code fragments because of monocular usage
231  //cv::Mat center = (cv::Mat_<float>(4, 1) << mHalfBaseline, 0, 0, 1);
232  //Cw = Twc*center;
233 }

◆ setTexturePath()

void WAIKeyFrame::setTexturePath ( const std::string &  path)
inline

Definition at line 291 of file WAIKeyFrame.h.

291 { _pathToTexture = path; }

◆ TrackedMapPoints()

int WAIKeyFrame::TrackedMapPoints ( const int &  minObs)

Definition at line 398 of file WAIKeyFrame.cpp.

399 {
400  unique_lock<mutex> lock(mMutexFeatures);
401 
402  int nPoints = 0;
403  const bool bCheckObs = minObs > 0;
404  for (int i = 0; i < N; i++)
405  {
406  WAIMapPoint* pMP = mvpMapPoints[i];
407  if (pMP)
408  {
409  if (!pMP->isBad())
410  {
411  if (bCheckObs)
412  {
413  if (mvpMapPoints[i]->Observations() >= minObs)
414  nPoints++;
415  }
416  else
417  nPoints++;
418  }
419  }
420  }
421 
422  return nPoints;
423 }

◆ UpdateBestCovisibles()

void WAIKeyFrame::UpdateBestCovisibles ( )

Definition at line 280 of file WAIKeyFrame.cpp.

281 {
282  unique_lock<mutex> lock(mMutexConnections);
283  vector<pair<int, WAIKeyFrame*>> vPairs;
284  vPairs.reserve(mConnectedKeyFrameWeights.size());
285  for (map<WAIKeyFrame*, int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend = mConnectedKeyFrameWeights.end(); mit != mend; mit++)
286  vPairs.push_back(make_pair(mit->second, mit->first));
287 
288  sort(vPairs.begin(), vPairs.end());
289  list<WAIKeyFrame*> lKFs;
290  list<int> lWs;
291  for (size_t i = 0, iend = vPairs.size(); i < iend; i++)
292  {
293  lKFs.push_front(vPairs[i].second);
294  lWs.push_front(vPairs[i].first);
295  }
296 
297  mvpOrderedConnectedKeyFrames = vector<WAIKeyFrame*>(lKFs.begin(), lKFs.end());
298  mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
299 }

◆ UpdateConnections()

void WAIKeyFrame::UpdateConnections ( std::map< WAIKeyFrame *, int >  KFcounter,
bool  buildSpanningTree 
)

Definition at line 475 of file WAIKeyFrame.cpp.

476 {
477  // This should not happen
478  if (KFcounter.empty())
479  return;
480 
481  //If the counter is greater than threshold add connection
482  //In case no keyframe counter is over threshold add the one with maximum counter
483  int nmax = 0;
484  WAIKeyFrame* pKFmax = NULL;
485  int th = 15;
486 
487  vector<pair<int, WAIKeyFrame*>> vPairs;
488  vPairs.reserve(KFcounter.size());
489  for (map<WAIKeyFrame*, int>::iterator mit = KFcounter.begin(), mend = KFcounter.end(); mit != mend; mit++)
490  {
491  if (mit->second > nmax)
492  {
493  nmax = mit->second;
494  pKFmax = mit->first;
495  }
496  if (mit->second >= th)
497  {
498  vPairs.push_back(make_pair(mit->second, mit->first));
499  (mit->first)->AddConnection(this, mit->second);
500  }
501  }
502 
503  if (vPairs.empty())
504  {
505  vPairs.push_back(make_pair(nmax, pKFmax));
506  pKFmax->AddConnection(this, nmax);
507  }
508 
509  sort(vPairs.begin(), vPairs.end());
510  list<WAIKeyFrame*> lKFs;
511  list<int> lWs;
512  for (size_t i = 0; i < vPairs.size(); i++)
513  {
514  lKFs.push_front(vPairs[i].second);
515  lWs.push_front(vPairs[i].first);
516  }
517 
518  {
519  unique_lock<mutex> lockCon(mMutexConnections);
520 
521  // mspConnectedKeyFrames = spConnectedKeyFrames;
522  mConnectedKeyFrameWeights = KFcounter;
523  mvpOrderedConnectedKeyFrames = vector<WAIKeyFrame*>(lKFs.begin(), lKFs.end());
524  mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
525 
526  if (mbFirstConnection && mnId != 0)
527  {
528  if (buildSpanningTree)
529  {
531  mpParent->AddChild(this);
532  }
533  mbFirstConnection = false;
534  }
535  }
536 }
void AddConnection(WAIKeyFrame *pKF, int weight)

◆ weightComp()

static bool WAIKeyFrame::weightComp ( int  a,
int  b 
)
inlinestatic

Definition at line 158 of file WAIKeyFrame.h.

159  {
160  return a > b;
161  }

Member Data Documentation

◆ _fixed

const bool WAIKeyFrame::_fixed = false

Definition at line 181 of file WAIKeyFrame.h.

◆ _pathToTexture

std::string WAIKeyFrame::_pathToTexture
private

Definition at line 308 of file WAIKeyFrame.h.

◆ _Tcw

cv::Mat WAIKeyFrame::_Tcw
protected

Definition at line 257 of file WAIKeyFrame.h.

◆ _Twc

cv::Mat WAIKeyFrame::_Twc
protected

opencv coordinate representation: z-axis points to principlal point, x-axis to the right and y-axis down Infos about the pose: https://github.com/raulmur/ORB_SLAM2/issues/249

Definition at line 256 of file WAIKeyFrame.h.

◆ cx

const float WAIKeyFrame::cx

Definition at line 214 of file WAIKeyFrame.h.

◆ cy

const float WAIKeyFrame::cy

Definition at line 214 of file WAIKeyFrame.h.

◆ fx

const float WAIKeyFrame::fx

Definition at line 214 of file WAIKeyFrame.h.

◆ fy

const float WAIKeyFrame::fy

Definition at line 214 of file WAIKeyFrame.h.

◆ imgGray

cv::Mat WAIKeyFrame::imgGray

Definition at line 248 of file WAIKeyFrame.h.

◆ invfx

const float WAIKeyFrame::invfx

Definition at line 214 of file WAIKeyFrame.h.

◆ invfy

const float WAIKeyFrame::invfy

Definition at line 214 of file WAIKeyFrame.h.

◆ mbBad

bool WAIKeyFrame::mbBad
protected

Definition at line 283 of file WAIKeyFrame.h.

◆ mbFirstConnection

bool WAIKeyFrame::mbFirstConnection = true
protected

Definition at line 275 of file WAIKeyFrame.h.

◆ mbNotErase

bool WAIKeyFrame::mbNotErase
protected

Definition at line 281 of file WAIKeyFrame.h.

◆ mBowVec

WAIBowVector WAIKeyFrame::mBowVec

Definition at line 226 of file WAIKeyFrame.h.

◆ mbToBeErased

bool WAIKeyFrame::mbToBeErased
protected

Definition at line 282 of file WAIKeyFrame.h.

◆ mConnectedKeyFrameWeights

std::map<WAIKeyFrame*, int> WAIKeyFrame::mConnectedKeyFrameWeights
protected

Definition at line 269 of file WAIKeyFrame.h.

◆ mDescriptors

const cv::Mat WAIKeyFrame::mDescriptors

Definition at line 223 of file WAIKeyFrame.h.

◆ mFeatVec

WAIFeatVector WAIKeyFrame::mFeatVec

Definition at line 227 of file WAIKeyFrame.h.

◆ mfGridElementHeightInv

const float WAIKeyFrame::mfGridElementHeightInv

Definition at line 187 of file WAIKeyFrame.h.

◆ mfGridElementWidthInv

const float WAIKeyFrame::mfGridElementWidthInv

Definition at line 186 of file WAIKeyFrame.h.

◆ mfLogScaleFactor

const float WAIKeyFrame::mfLogScaleFactor

Definition at line 235 of file WAIKeyFrame.h.

◆ mfScaleFactor

const float WAIKeyFrame::mfScaleFactor

Definition at line 234 of file WAIKeyFrame.h.

◆ mGrid

std::vector<std::size_t> WAIKeyFrame::mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]
protected

Definition at line 266 of file WAIKeyFrame.h.

◆ mK

const cv::Mat WAIKeyFrame::mK

Definition at line 245 of file WAIKeyFrame.h.

◆ mLoopScore

float WAIKeyFrame::mLoopScore = -1.0

Definition at line 204 of file WAIKeyFrame.h.

◆ mMutexConnections

std::mutex WAIKeyFrame::mMutexConnections

Definition at line 287 of file WAIKeyFrame.h.

◆ mMutexFeatures

std::mutex WAIKeyFrame::mMutexFeatures

Definition at line 288 of file WAIKeyFrame.h.

◆ mMutexPose

std::mutex WAIKeyFrame::mMutexPose

Definition at line 286 of file WAIKeyFrame.h.

◆ mnFrameId

const long unsigned int WAIKeyFrame::mnFrameId

Definition at line 176 of file WAIKeyFrame.h.

◆ mnGridCols

const int WAIKeyFrame::mnGridCols

Definition at line 184 of file WAIKeyFrame.h.

◆ mnGridRows

const int WAIKeyFrame::mnGridRows

Definition at line 185 of file WAIKeyFrame.h.

◆ mnId

long unsigned int WAIKeyFrame::mnId

Definition at line 175 of file WAIKeyFrame.h.

◆ mnLoopQuery

long unsigned int WAIKeyFrame::mnLoopQuery = 0

Definition at line 202 of file WAIKeyFrame.h.

◆ mnLoopWords

int WAIKeyFrame::mnLoopWords = 0

Definition at line 203 of file WAIKeyFrame.h.

◆ mnMarker

long unsigned int WAIKeyFrame::mnMarker[7]

Definition at line 199 of file WAIKeyFrame.h.

◆ mnMaxX

const int WAIKeyFrame::mnMaxX

Definition at line 243 of file WAIKeyFrame.h.

◆ mnMaxY

const int WAIKeyFrame::mnMaxY

Definition at line 244 of file WAIKeyFrame.h.

◆ mnMinX

const int WAIKeyFrame::mnMinX

Definition at line 241 of file WAIKeyFrame.h.

◆ mnMinY

const int WAIKeyFrame::mnMinY

Definition at line 242 of file WAIKeyFrame.h.

◆ mnRelocQuery

long unsigned int WAIKeyFrame::mnRelocQuery = 0

Definition at line 205 of file WAIKeyFrame.h.

◆ mnRelocWords

int WAIKeyFrame::mnRelocWords = 0

Definition at line 206 of file WAIKeyFrame.h.

◆ mnScaleLevels

const int WAIKeyFrame::mnScaleLevels

Definition at line 233 of file WAIKeyFrame.h.

◆ mpParent

WAIKeyFrame* WAIKeyFrame::mpParent = NULL
protected

Definition at line 276 of file WAIKeyFrame.h.

◆ mRelocScore

float WAIKeyFrame::mRelocScore = -1.0f

Definition at line 207 of file WAIKeyFrame.h.

◆ mspChildrens

std::set<WAIKeyFrame*> WAIKeyFrame::mspChildrens
protected

Definition at line 277 of file WAIKeyFrame.h.

◆ mspLoopEdges

std::set<WAIKeyFrame*> WAIKeyFrame::mspLoopEdges
protected

Definition at line 278 of file WAIKeyFrame.h.

◆ mTcp

cv::Mat WAIKeyFrame::mTcp

Definition at line 230 of file WAIKeyFrame.h.

◆ mTcwGBA

cv::Mat WAIKeyFrame::mTcwGBA

Definition at line 210 of file WAIKeyFrame.h.

◆ mTcwRefGBA

cv::Mat WAIKeyFrame::mTcwRefGBA

Definition at line 211 of file WAIKeyFrame.h.

◆ mTimeStamp

const double WAIKeyFrame::mTimeStamp

Definition at line 178 of file WAIKeyFrame.h.

◆ mvInvLevelSigma2

const std::vector<float> WAIKeyFrame::mvInvLevelSigma2

Definition at line 238 of file WAIKeyFrame.h.

◆ mvKeysUn

const std::vector<cv::KeyPoint> WAIKeyFrame::mvKeysUn

Definition at line 220 of file WAIKeyFrame.h.

◆ mvLevelSigma2

const std::vector<float> WAIKeyFrame::mvLevelSigma2

Definition at line 237 of file WAIKeyFrame.h.

◆ mvOrderedWeights

std::vector<int> WAIKeyFrame::mvOrderedWeights
protected

Definition at line 272 of file WAIKeyFrame.h.

◆ mvpMapPoints

std::vector<WAIMapPoint*> WAIKeyFrame::mvpMapPoints
protected

Definition at line 263 of file WAIKeyFrame.h.

◆ mvpOrderedConnectedKeyFrames

std::vector<WAIKeyFrame*> WAIKeyFrame::mvpOrderedConnectedKeyFrames
protected

Definition at line 271 of file WAIKeyFrame.h.

◆ mvScaleFactors

const std::vector<float> WAIKeyFrame::mvScaleFactors

Definition at line 236 of file WAIKeyFrame.h.

◆ N

const int WAIKeyFrame::N = 0

Definition at line 217 of file WAIKeyFrame.h.

◆ nNextId

long unsigned int WAIKeyFrame::nNextId = 0
static

This file is part of ORB-SLAM2.

Copyright (C) 2014-2016 Ra�l Mur-Artal <raulmur at unizar dot es> (University of Zaragoza) For more information see https://github.com/raulmur/ORB_SLAM2

ORB-SLAM2 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.

ORB-SLAM2 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with ORB-SLAM2. If not, see http://www.gnu.org/licenses/.

Definition at line 174 of file WAIKeyFrame.h.

◆ Ow

cv::Mat WAIKeyFrame::Ow
protected

camera center

Definition at line 259 of file WAIKeyFrame.h.


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