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

#include <WAIMapPoint.h>

Public Types

enum  RefKfSource { RefKfSource_None = 0 , RefKfSource_Constructor = 1 , RefKfSource_EraseObservation = 2 }
 

Public Member Functions

 WAIMapPoint (int id, const cv::Mat &Pos, bool fixMp)
 constructor used during map loading More...
 
 WAIMapPoint (const cv::Mat &Pos, WAIKeyFrame *pRefKF)
 
WAI::V3 worldPosVec ()
 
WAI::V3 normalVec ()
 
void worldPosVec (WAI::V3)
 
void SetWorldPos (const cv::Mat &Pos)
 
cv::Mat GetWorldPos ()
 
cv::Mat GetNormal ()
 
void SetNormal (const cv::Mat &normal)
 
WAIKeyFrameGetReferenceKeyFrame ()
 
std::map< WAIKeyFrame *, size_t > GetObservations ()
 
int Observations ()
 
void AddObservation (WAIKeyFrame *pKF, size_t idx)
 
void EraseObservation (WAIKeyFrame *pKF)
 
int GetIndexInKeyFrame (WAIKeyFrame *pKF)
 
bool IsInKeyFrame (WAIKeyFrame *pKF)
 
void SetBadFlag ()
 
bool isBad ()
 
void Replace (WAIMapPoint *pMP)
 
WAIMapPointGetReplaced ()
 
void IncreaseVisible (int n=1)
 
void IncreaseFound (int n=1)
 
float GetFoundRatio ()
 
void ComputeDistinctiveDescriptors ()
 
cv::Mat GetDescriptor ()
 
void SetDescriptor (const cv::Mat &descriptor)
 
void UpdateNormalAndDepth ()
 
float GetMinDistanceInvariance ()
 
float GetMaxDistanceInvariance ()
 
int PredictScale (const float &currentDist, WAIKeyFrame *pF)
 
int PredictScale (const float &currentDist, WAIFrame *pF)
 
WAIKeyFramerefKf () const
 
void refKf (WAIKeyFrame *refKf)
 
size_t getSizeOfCvMat (const cv::Mat &mat)
 
size_t getSizeOf ()
 
bool isFixed () const
 
bool loadedFromMap ()
 
float GetMaxDistance ()
 
float GetMinDistance ()
 
void SetMaxDistance (float maxDist)
 
void SetMinDistance (float minDist)
 

Public Attributes

RefKfSource refKfSource = RefKfSource_None
 
long unsigned int mnId = -1
 
long int mnFirstKFid
 
int nObs = 0
 
float mTrackProjX = 0.0f
 
float mTrackProjY = 0.0f
 
bool mbTrackInView = false
 
int mnTrackScaleLevel = 0
 
float mTrackViewCos = 0.0f
 
long unsigned int mnLastFrameSeen = 0
 
int mnMarker [7]
 
long unsigned int mnCorrectedByKF
 
long unsigned int mnCorrectedReference
 
cv::Mat mPosGBA
 

Static Public Attributes

static long unsigned int nNextId = 0
 
static std::mutex mGlobalMutex
 
static std::mutex mMutexMapPointCreation
 

Protected Attributes

bool _fixed = false
 
bool _loadedFromMap = false
 
cv::Mat mWorldPos
 
std::map< WAIKeyFrame *, size_t > mObservations
 
cv::Mat mNormalVector
 
cv::Mat mDescriptor
 
WAIKeyFramempRefKF = NULL
 
int mnVisible = 0
 
int mnFound = 0
 
bool mbBad = false
 
WAIMapPointmpReplaced
 
float mfMinDistance = 0.f
 
float mfMaxDistance = 0.f
 
std::mutex mMutexPos
 
std::mutex mMutexFeatures
 

Detailed Description

Definition at line 39 of file WAIMapPoint.h.

Member Enumeration Documentation

◆ RefKfSource

Enumerator
RefKfSource_None 
RefKfSource_Constructor 
RefKfSource_EraseObservation 

Definition at line 93 of file WAIMapPoint.h.

94  {
95  RefKfSource_None = 0,
98  };
@ RefKfSource_Constructor
Definition: WAIMapPoint.h:96
@ RefKfSource_EraseObservation
Definition: WAIMapPoint.h:97
@ RefKfSource_None
Definition: WAIMapPoint.h:95

Constructor & Destructor Documentation

◆ WAIMapPoint() [1/2]

WAIMapPoint::WAIMapPoint ( int  id,
const cv::Mat &  Pos,
bool  fixMp 
)

constructor used during map loading

Definition at line 22 of file WAIMapPoint.cpp.

23  : mnId(id),
24  mnFirstKFid(-1),
25  /* mnFirstFrame(pRefKF->mnFrameId), */ nObs(0),
26  mnLastFrameSeen(0),
27  mnCorrectedByKF(0),
29  mpRefKF(NULL),
30  mnVisible(1),
31  mnFound(1),
32  mbBad(false),
33  mpReplaced(static_cast<WAIMapPoint*>(NULL)),
34  mfMinDistance(0),
35  mfMaxDistance(0),
36  _fixed(fixMp),
37  _loadedFromMap(true)
38 {
39 
40  mnMarker[0] = 0;
41  mnMarker[1] = 0;
42  mnMarker[2] = 0;
43  mnMarker[3] = 0;
44  mnMarker[4] = 0;
45  mnMarker[5] = 0;
46  mnMarker[6] = 0;
47 
48  SetWorldPos(Pos);
49  mNormalVector = cv::Mat::zeros(3, 1, CV_32F);
50 
51  //update highest used id for new map point generation
52  if (id >= (int)nNextId)
53  nNextId = id + 1;
54 }
long unsigned int mnId
Definition: WAIMapPoint.h:108
static long unsigned int nNextId
Definition: WAIMapPoint.h:110
float mfMaxDistance
Definition: WAIMapPoint.h:178
cv::Mat mNormalVector
Definition: WAIMapPoint.h:160
long unsigned int mnCorrectedReference
Definition: WAIMapPoint.h:133
long int mnFirstKFid
Definition: WAIMapPoint.h:111
WAIKeyFrame * mpRefKF
Definition: WAIMapPoint.h:166
WAIMapPoint * mpReplaced
Definition: WAIMapPoint.h:174
long unsigned int mnLastFrameSeen
Definition: WAIMapPoint.h:124
float mfMinDistance
Definition: WAIMapPoint.h:177
bool _loadedFromMap
Definition: WAIMapPoint.h:150
int mnMarker[7]
Definition: WAIMapPoint.h:128
void SetWorldPos(const cv::Mat &Pos)
long unsigned int mnCorrectedByKF
Definition: WAIMapPoint.h:132

◆ WAIMapPoint() [2/2]

WAIMapPoint::WAIMapPoint ( const cv::Mat &  Pos,
WAIKeyFrame pRefKF 
)

Definition at line 56 of file WAIMapPoint.cpp.

58  : mnFirstKFid(pRefKF->mnId),
59  /* mnFirstFrame(pRefKF->mnFrameId), */
60  nObs(0),
61  mnLastFrameSeen(0),
62  mnCorrectedByKF(0),
64  mpRefKF(pRefKF),
65  mnVisible(1),
66  mnFound(1),
67  mbBad(false),
68  mpReplaced(static_cast<WAIMapPoint*>(NULL)),
69  mfMinDistance(0),
70  mfMaxDistance(0),
71  _fixed(false),
72  _loadedFromMap(false)
73 {
74  mnMarker[0] = 0;
75  mnMarker[1] = 0;
76  mnMarker[2] = 0;
77  mnMarker[3] = 0;
78  mnMarker[4] = 0;
79  mnMarker[5] = 0;
80  mnMarker[6] = 0;
81 
82  SetWorldPos(Pos);
83  //Pos.copyTo(mWorldPos);
84  mNormalVector = cv::Mat::zeros(3, 1, CV_32F);
85 
86  //TODO(Luluc) remove mMutexPointCreaton on WAIMap
87  // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
88  unique_lock<mutex> lock(mMutexMapPointCreation);
89  mnId = nNextId++;
90 
92 }
long unsigned int mnId
Definition: WAIKeyFrame.h:175
RefKfSource refKfSource
Definition: WAIMapPoint.h:100
static std::mutex mMutexMapPointCreation
Definition: WAIMapPoint.h:138

Member Function Documentation

◆ AddObservation()

void WAIMapPoint::AddObservation ( WAIKeyFrame pKF,
size_t  idx 
)

Definition at line 157 of file WAIMapPoint.cpp.

158 {
159  unique_lock<mutex> lock(mMutexFeatures);
160  if (mObservations.count(pKF))
161  return;
162  mObservations[pKF] = idx;
163  nObs++;
164 }
std::mutex mMutexFeatures
Definition: WAIMapPoint.h:181
std::map< WAIKeyFrame *, size_t > mObservations
Definition: WAIMapPoint.h:157

◆ ComputeDistinctiveDescriptors()

void WAIMapPoint::ComputeDistinctiveDescriptors ( )

Definition at line 313 of file WAIMapPoint.cpp.

314 {
315  // Retrieve all observed descriptors
316  vector<cv::Mat> vDescriptors;
317 
318  map<WAIKeyFrame*, size_t> observations;
319 
320  {
321  unique_lock<mutex> lock1(mMutexFeatures);
322  if (mbBad)
323  return;
324  observations = mObservations;
325  }
326 
327  if (observations.empty())
328  return;
329 
330  vDescriptors.reserve(observations.size());
331 
332  for (map<WAIKeyFrame*, size_t>::iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++)
333  {
334  WAIKeyFrame* pKF = mit->first;
335 
336  if (pKF && !pKF->isBad())
337  vDescriptors.push_back(pKF->mDescriptors.row((int)mit->second));
338  }
339 
340  if (vDescriptors.empty())
341  return;
342 
343  // Compute distances between them
344 #ifdef _WINDOWS
345  size_t N = vDescriptors.size();
346 
347  float** Distances = new float*[N];
348  for (size_t i = 0; i < N; ++i)
349  Distances[i] = new float[N];
350 
351  for (size_t i = 0; i < N; i++)
352  {
353  Distances[i][i] = 0;
354  for (size_t j = i + 1; j < N; j++)
355  {
356  int distij = ORBmatcher::DescriptorDistance(vDescriptors[i], vDescriptors[j]);
357  Distances[i][j] = (float)distij;
358  Distances[j][i] = (float)distij;
359  }
360  }
361 #else
362  const size_t N = vDescriptors.size();
363 
364  float Distances[N][N];
365  for (size_t i = 0; i < N; i++)
366  {
367  Distances[i][i] = 0;
368  for (size_t j = i + 1; j < N; j++)
369  {
370  int distij = ORBmatcher::DescriptorDistance(vDescriptors[i], vDescriptors[j]);
371  Distances[i][j] = distij;
372  Distances[j][i] = distij;
373  }
374  }
375 #endif
376  // Take the descriptor with least median distance to the rest
377  int BestMedian = INT_MAX;
378  int BestIdx = 0;
379  for (size_t i = 0; i < N; i++)
380  {
381  vector<int> vDists(Distances[i], Distances[i] + N);
382  sort(vDists.begin(), vDists.end());
383  int median = vDists[(uint64_t)(0.5 * (N - 1))];
384 
385  if (median < BestMedian)
386  {
387  BestMedian = median;
388  BestIdx = (int)i;
389  }
390  }
391 
392  //free Distances
393 #ifdef _WINDOWS
394  for (size_t i = 0; i < N; ++i)
395  delete Distances[i];
396  delete[] Distances;
397 #endif
398 
399  {
400  unique_lock<mutex> lock(mMutexFeatures);
401  mDescriptor = vDescriptors[BestIdx].clone();
402  }
403 }
AR Keyframe node class.
Definition: WAIKeyFrame.h:60
const cv::Mat mDescriptors
Definition: WAIKeyFrame.h:223
cv::Mat mDescriptor
Definition: WAIMapPoint.h:163

◆ EraseObservation()

void WAIMapPoint::EraseObservation ( WAIKeyFrame pKF)

Definition at line 166 of file WAIMapPoint.cpp.

167 {
168  bool bBad = false;
169  {
170  unique_lock<mutex> lock(mMutexFeatures);
171  if (mObservations.count(pKF))
172  {
173  //int idx = mObservations[pKF];
174  //if (pKF->mvuRight[idx] >= 0)
175  // nObs -= 2;
176  //else
177  // nObs--;
178  nObs--;
179 
180  mObservations.erase(pKF);
181 
182  if (mpRefKF == pKF)
183  {
184  for (auto it = mObservations.begin(); it != mObservations.end(); it++)
185  {
186  WAIKeyFrame* kf = it->first;
187  if (!kf->isBad())
188  {
189  mpRefKF = kf;
190  break;
191  }
192  else
193  {
194  continue;
195  }
196  }
197  if (mpRefKF == pKF)
198  {
199  bBad = true;
200  }
202  }
203 
204  // If only 2 observations or less, discard point
205  if (nObs <= 2)
206  bBad = true;
207  }
208  }
209 
210  if (bBad)
211  SetBadFlag();
212 }
void SetBadFlag()

◆ GetDescriptor()

cv::Mat WAIMapPoint::GetDescriptor ( )

Definition at line 405 of file WAIMapPoint.cpp.

406 {
407  unique_lock<mutex> lock(mMutexFeatures);
408  return mDescriptor.clone();
409 }

◆ GetFoundRatio()

float WAIMapPoint::GetFoundRatio ( )

Definition at line 307 of file WAIMapPoint.cpp.

308 {
309  unique_lock<mutex> lock(mMutexFeatures);
310  return static_cast<float>(mnFound) / mnVisible;
311 }

◆ GetIndexInKeyFrame()

int WAIMapPoint::GetIndexInKeyFrame ( WAIKeyFrame pKF)

Definition at line 417 of file WAIMapPoint.cpp.

418 {
419  unique_lock<mutex> lock(mMutexFeatures);
420  if (mObservations.count(pKF))
421  return (int)mObservations[pKF];
422  else
423  return -1;
424 }

◆ GetMaxDistance()

float WAIMapPoint::GetMaxDistance ( )

Definition at line 493 of file WAIMapPoint.cpp.

494 {
495  return mfMaxDistance;
496 }

◆ GetMaxDistanceInvariance()

float WAIMapPoint::GetMaxDistanceInvariance ( )

Definition at line 482 of file WAIMapPoint.cpp.

483 {
484  unique_lock<mutex> lock(mMutexPos);
485  return 1.2f * mfMaxDistance;
486 }
std::mutex mMutexPos
Definition: WAIMapPoint.h:180

◆ GetMinDistance()

float WAIMapPoint::GetMinDistance ( )

Definition at line 488 of file WAIMapPoint.cpp.

489 {
490  return mfMinDistance;
491 }

◆ GetMinDistanceInvariance()

float WAIMapPoint::GetMinDistanceInvariance ( )

Definition at line 476 of file WAIMapPoint.cpp.

477 {
478  unique_lock<mutex> lock(mMutexPos);
479  return 0.8f * mfMinDistance;
480 }

◆ GetNormal()

cv::Mat WAIMapPoint::GetNormal ( )

Definition at line 139 of file WAIMapPoint.cpp.

140 {
141  unique_lock<mutex> lock(mMutexPos);
142  return mNormalVector.clone();
143 }

◆ GetObservations()

std::map< WAIKeyFrame *, size_t > WAIMapPoint::GetObservations ( )

Definition at line 214 of file WAIMapPoint.cpp.

215 {
216  unique_lock<mutex> lock(mMutexFeatures);
217  return mObservations;
218 }

◆ GetReferenceKeyFrame()

WAIKeyFrame * WAIMapPoint::GetReferenceKeyFrame ( )

Definition at line 151 of file WAIMapPoint.cpp.

152 {
153  unique_lock<mutex> lock(mMutexFeatures);
154  return mpRefKF;
155 }

◆ GetReplaced()

WAIMapPoint * WAIMapPoint::GetReplaced ( )

Definition at line 243 of file WAIMapPoint.cpp.

244 {
245  unique_lock<mutex> lock1(mMutexFeatures);
246  unique_lock<mutex> lock2(mMutexPos);
247  return mpReplaced;
248 }

◆ getSizeOf()

size_t WAIMapPoint::getSizeOf ( )

Definition at line 554 of file WAIMapPoint.cpp.

555 {
556  size_t size = 0;
557 
558  size += sizeof(*this);
559  size += getSizeOfCvMat(mWorldPos);
560  size += getSizeOfCvMat(mNormalVector);
561  size += getSizeOfCvMat(mDescriptor);
562  size += getSizeOfCvMat(mPosGBA);
563 
564  return size;
565 }
cv::Mat mWorldPos
Definition: WAIMapPoint.h:154
size_t getSizeOfCvMat(const cv::Mat &mat)
cv::Mat mPosGBA
Definition: WAIMapPoint.h:134

◆ getSizeOfCvMat()

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

Definition at line 542 of file WAIMapPoint.cpp.

543 {
544  size_t size = 0;
545  if (mat.isContinuous())
546  size = mat.total() * mat.elemSize();
547  else
548  {
549  size = mat.step[0] * mat.rows;
550  }
551  return size;
552 }

◆ GetWorldPos()

cv::Mat WAIMapPoint::GetWorldPos ( )

Definition at line 133 of file WAIMapPoint.cpp.

134 {
135  unique_lock<mutex> lock(mMutexPos);
136  return mWorldPos.clone();
137 }

◆ IncreaseFound()

void WAIMapPoint::IncreaseFound ( int  n = 1)

Definition at line 301 of file WAIMapPoint.cpp.

302 {
303  unique_lock<mutex> lock(mMutexFeatures);
304  mnFound += n;
305 }

◆ IncreaseVisible()

void WAIMapPoint::IncreaseVisible ( int  n = 1)

Definition at line 295 of file WAIMapPoint.cpp.

296 {
297  unique_lock<mutex> lock(mMutexFeatures);
298  mnVisible += n;
299 }

◆ isBad()

bool WAIMapPoint::isBad ( )

Definition at line 288 of file WAIMapPoint.cpp.

289 {
290  unique_lock<mutex> lock(mMutexFeatures);
291  unique_lock<mutex> lock2(mMutexPos);
292  return mbBad;
293 }

◆ isFixed()

bool WAIMapPoint::isFixed ( ) const
inline

Definition at line 105 of file WAIMapPoint.h.

105 { return _fixed; }

◆ IsInKeyFrame()

bool WAIMapPoint::IsInKeyFrame ( WAIKeyFrame pKF)

Definition at line 426 of file WAIMapPoint.cpp.

427 {
428  unique_lock<mutex> lock(mMutexFeatures);
429  return (mObservations.count(pKF));
430 }

◆ loadedFromMap()

bool WAIMapPoint::loadedFromMap ( )
inline

Definition at line 106 of file WAIMapPoint.h.

106 { return _loadedFromMap; }

◆ normalVec()

WAI::V3 WAIMapPoint::normalVec ( )

Definition at line 113 of file WAIMapPoint.cpp.

114 {
115  WAI::V3 vec = {};
116 
117  if (!mNormalVector.empty())
118  {
119  vec.x = mNormalVector.at<float>(0, 0);
120  vec.y = mNormalVector.at<float>(1, 0);
121  vec.z = mNormalVector.at<float>(2, 0);
122  }
123  return vec;
124 }
float x
Definition: WAIMath.h:33
float z
Definition: WAIMath.h:33
float y
Definition: WAIMath.h:33

◆ Observations()

int WAIMapPoint::Observations ( )

Definition at line 220 of file WAIMapPoint.cpp.

221 {
222  unique_lock<mutex> lock(mMutexFeatures);
223  return nObs;
224 }

◆ PredictScale() [1/2]

int WAIMapPoint::PredictScale ( const float &  currentDist,
WAIFrame pF 
)

Definition at line 525 of file WAIMapPoint.cpp.

526 {
527  float ratio;
528  {
529  unique_lock<mutex> lock(mMutexPos);
530  ratio = mfMaxDistance / currentDist;
531  }
532 
533  int nScale = (int)ceil(log(ratio) / pF->mfLogScaleFactor);
534  if (nScale < 0)
535  nScale = 0;
536  else if (nScale >= pF->mnScaleLevels)
537  nScale = pF->mnScaleLevels - 1;
538 
539  return nScale;
540 }
int mnScaleLevels
Definition: WAIFrame.h:168
float mfLogScaleFactor
Definition: WAIFrame.h:170
T ceil(T a)
Definition: Utils.h:247
void log(const char *tag, const char *format,...)
logs a formatted string platform independently
Definition: Utils.cpp:1103

◆ PredictScale() [2/2]

int WAIMapPoint::PredictScale ( const float &  currentDist,
WAIKeyFrame pF 
)

Definition at line 508 of file WAIMapPoint.cpp.

509 {
510  float ratio;
511  {
512  unique_lock<mutex> lock(mMutexPos);
513  ratio = mfMaxDistance / currentDist;
514  }
515 
516  int nScale = (int)ceil(log(ratio) / pKF->mfLogScaleFactor);
517  if (nScale < 0)
518  nScale = 0;
519  else if (nScale >= pKF->mnScaleLevels)
520  nScale = pKF->mnScaleLevels - 1;
521 
522  return nScale;
523 }

◆ refKf() [1/2]

WAIKeyFrame* WAIMapPoint::refKf ( ) const
inline

Definition at line 90 of file WAIMapPoint.h.

90 { return mpRefKF; }

◆ refKf() [2/2]

void WAIMapPoint::refKf ( WAIKeyFrame refKf)
inline

Definition at line 91 of file WAIMapPoint.h.

91 { mpRefKF = refKf; }
WAIKeyFrame * refKf() const
Definition: WAIMapPoint.h:90

◆ Replace()

void WAIMapPoint::Replace ( WAIMapPoint pMP)

Definition at line 250 of file WAIMapPoint.cpp.

251 {
252  if (pMP->mnId == this->mnId)
253  return;
254 
255  int nvisible, nfound;
256  map<WAIKeyFrame*, size_t> obs;
257  {
258  unique_lock<mutex> lock1(mMutexFeatures);
259  unique_lock<mutex> lock2(mMutexPos);
260  obs = mObservations;
261  mObservations.clear();
262  mbBad = true;
263  nvisible = mnVisible;
264  nfound = mnFound;
265  mpReplaced = pMP;
266  }
267 
268  for (map<WAIKeyFrame*, size_t>::iterator mit = obs.begin(), mend = obs.end(); mit != mend; mit++)
269  {
270  // Replace measurement in keyframe
271  WAIKeyFrame* pKF = mit->first;
272 
273  if (!pMP->IsInKeyFrame(pKF))
274  {
275  pKF->ReplaceMapPointMatch(mit->second, pMP);
276  pMP->AddObservation(pKF, mit->second);
277  }
278  else
279  {
280  pKF->EraseMapPointMatch(mit->second);
281  }
282  }
283  pMP->IncreaseFound(nfound);
284  pMP->IncreaseVisible(nvisible);
286 }
void EraseMapPointMatch(WAIMapPoint *pMP)
void ReplaceMapPointMatch(const size_t &idx, WAIMapPoint *pMP)
void AddObservation(WAIKeyFrame *pKF, size_t idx)
bool IsInKeyFrame(WAIKeyFrame *pKF)
void IncreaseVisible(int n=1)
void ComputeDistinctiveDescriptors()
void IncreaseFound(int n=1)

◆ SetBadFlag()

void WAIMapPoint::SetBadFlag ( )

Definition at line 226 of file WAIMapPoint.cpp.

227 {
228  map<WAIKeyFrame*, size_t> obs;
229  {
230  unique_lock<mutex> lock1(mMutexFeatures);
231  unique_lock<mutex> lock2(mMutexPos);
232  mbBad = true;
233  obs = mObservations;
234  mObservations.clear();
235  }
236  for (map<WAIKeyFrame*, size_t>::iterator mit = obs.begin(), mend = obs.end(); mit != mend; mit++)
237  {
238  WAIKeyFrame* pKF = mit->first;
239  pKF->EraseMapPointMatch(mit->second);
240  }
241 }

◆ SetDescriptor()

void WAIMapPoint::SetDescriptor ( const cv::Mat &  descriptor)

Definition at line 411 of file WAIMapPoint.cpp.

412 {
413  unique_lock<mutex> lock(mMutexFeatures);
414  descriptor.copyTo(mDescriptor);
415 }

◆ SetMaxDistance()

void WAIMapPoint::SetMaxDistance ( float  maxDist)

Definition at line 498 of file WAIMapPoint.cpp.

499 {
500  mfMaxDistance = maxDist;
501 }

◆ SetMinDistance()

void WAIMapPoint::SetMinDistance ( float  minDist)

Definition at line 503 of file WAIMapPoint.cpp.

504 {
505  mfMinDistance = minDist;
506 }

◆ SetNormal()

void WAIMapPoint::SetNormal ( const cv::Mat &  normal)

Definition at line 145 of file WAIMapPoint.cpp.

146 {
147  unique_lock<mutex> lock(mMutexPos);
148  normal.copyTo(mNormalVector);
149 }

◆ SetWorldPos()

void WAIMapPoint::SetWorldPos ( const cv::Mat &  Pos)

Definition at line 126 of file WAIMapPoint.cpp.

127 {
128  unique_lock<mutex> lock2(mGlobalMutex);
129  unique_lock<mutex> lock(mMutexPos);
130  Pos.copyTo(mWorldPos);
131 }
static std::mutex mGlobalMutex
Definition: WAIMapPoint.h:137

◆ UpdateNormalAndDepth()

void WAIMapPoint::UpdateNormalAndDepth ( )

Definition at line 433 of file WAIMapPoint.cpp.

434 {
435  map<WAIKeyFrame*, size_t> observations;
436  WAIKeyFrame* pRefKF;
437  cv::Mat Pos;
438  {
439  unique_lock<mutex> lock1(mMutexFeatures);
440  unique_lock<mutex> lock2(mMutexPos);
441  if (mbBad)
442  return;
443  observations = mObservations;
444  pRefKF = mpRefKF;
445  Pos = mWorldPos.clone();
446  }
447 
448  if (observations.empty())
449  return;
450 
451  cv::Mat normal = cv::Mat::zeros(3, 1, CV_32F);
452  int n = 0;
453  for (map<WAIKeyFrame*, size_t>::iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++)
454  {
455  WAIKeyFrame* pKF = mit->first;
456  cv::Mat Owi = pKF->GetCameraCenter();
457  cv::Mat normali = mWorldPos - Owi;
458  normal = normal + normali / cv::norm(normali);
459  n++;
460  }
461 
462  cv::Mat PC = Pos - pRefKF->GetCameraCenter();
463  const float dist = (float)cv::norm(PC);
464  const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;
465  const float levelScaleFactor = pRefKF->mvScaleFactors[level];
466  const int nLevels = pRefKF->mnScaleLevels;
467 
468  {
469  unique_lock<mutex> lock3(mMutexPos);
470  mfMaxDistance = dist * levelScaleFactor;
471  mfMinDistance = mfMaxDistance / pRefKF->mvScaleFactors[nLevels - 1];
472  mNormalVector = normal / n;
473  }
474 }
cv::Mat GetCameraCenter()
const int mnScaleLevels
Definition: WAIKeyFrame.h:233
const std::vector< float > mvScaleFactors
Definition: WAIKeyFrame.h:236
const std::vector< cv::KeyPoint > mvKeysUn
Definition: WAIKeyFrame.h:220

◆ worldPosVec() [1/2]

WAI::V3 WAIMapPoint::worldPosVec ( )

Definition at line 95 of file WAIMapPoint.cpp.

96 {
97  unique_lock<mutex> lock(mMutexPos);
98  WAI::V3 vec;
99  vec.x = mWorldPos.at<float>(0, 0);
100  vec.y = mWorldPos.at<float>(1, 0);
101  vec.z = mWorldPos.at<float>(2, 0);
102  return vec;
103 }

◆ worldPosVec() [2/2]

void WAIMapPoint::worldPosVec ( WAI::V3  vec)

Definition at line 105 of file WAIMapPoint.cpp.

106 {
107  unique_lock<mutex> lock(mMutexPos);
108  mWorldPos.at<float>(0, 0) = vec.x;
109  mWorldPos.at<float>(1, 0) = vec.y;
110  mWorldPos.at<float>(2, 0) = vec.z;
111 }

Member Data Documentation

◆ _fixed

bool WAIMapPoint::_fixed = false
protected

Definition at line 148 of file WAIMapPoint.h.

◆ _loadedFromMap

bool WAIMapPoint::_loadedFromMap = false
protected

Definition at line 150 of file WAIMapPoint.h.

◆ mbBad

bool WAIMapPoint::mbBad = false
protected

Definition at line 173 of file WAIMapPoint.h.

◆ mbTrackInView

bool WAIMapPoint::mbTrackInView = false

Definition at line 120 of file WAIMapPoint.h.

◆ mDescriptor

cv::Mat WAIMapPoint::mDescriptor
protected

Definition at line 163 of file WAIMapPoint.h.

◆ mfMaxDistance

float WAIMapPoint::mfMaxDistance = 0.f
protected

Definition at line 178 of file WAIMapPoint.h.

◆ mfMinDistance

float WAIMapPoint::mfMinDistance = 0.f
protected

Definition at line 177 of file WAIMapPoint.h.

◆ mGlobalMutex

mutex WAIMapPoint::mGlobalMutex
static

Definition at line 137 of file WAIMapPoint.h.

◆ mMutexFeatures

std::mutex WAIMapPoint::mMutexFeatures
protected

Definition at line 181 of file WAIMapPoint.h.

◆ mMutexMapPointCreation

mutex WAIMapPoint::mMutexMapPointCreation
static

Definition at line 138 of file WAIMapPoint.h.

◆ mMutexPos

std::mutex WAIMapPoint::mMutexPos
protected

Definition at line 180 of file WAIMapPoint.h.

◆ mnCorrectedByKF

long unsigned int WAIMapPoint::mnCorrectedByKF

Definition at line 132 of file WAIMapPoint.h.

◆ mnCorrectedReference

long unsigned int WAIMapPoint::mnCorrectedReference

Definition at line 133 of file WAIMapPoint.h.

◆ mnFirstKFid

long int WAIMapPoint::mnFirstKFid

Definition at line 111 of file WAIMapPoint.h.

◆ mnFound

int WAIMapPoint::mnFound = 0
protected

Definition at line 170 of file WAIMapPoint.h.

◆ mnId

long unsigned int WAIMapPoint::mnId = -1

Definition at line 108 of file WAIMapPoint.h.

◆ mnLastFrameSeen

long unsigned int WAIMapPoint::mnLastFrameSeen = 0

Definition at line 124 of file WAIMapPoint.h.

◆ mnMarker

int WAIMapPoint::mnMarker[7]

Definition at line 128 of file WAIMapPoint.h.

◆ mNormalVector

cv::Mat WAIMapPoint::mNormalVector
protected

Definition at line 160 of file WAIMapPoint.h.

◆ mnTrackScaleLevel

int WAIMapPoint::mnTrackScaleLevel = 0

Definition at line 121 of file WAIMapPoint.h.

◆ mnVisible

int WAIMapPoint::mnVisible = 0
protected

Definition at line 169 of file WAIMapPoint.h.

◆ mObservations

std::map<WAIKeyFrame*, size_t> WAIMapPoint::mObservations
protected

Definition at line 157 of file WAIMapPoint.h.

◆ mPosGBA

cv::Mat WAIMapPoint::mPosGBA

Definition at line 134 of file WAIMapPoint.h.

◆ mpRefKF

WAIKeyFrame* WAIMapPoint::mpRefKF = NULL
protected

Definition at line 166 of file WAIMapPoint.h.

◆ mpReplaced

WAIMapPoint* WAIMapPoint::mpReplaced
protected

Definition at line 174 of file WAIMapPoint.h.

◆ mTrackProjX

float WAIMapPoint::mTrackProjX = 0.0f

Definition at line 116 of file WAIMapPoint.h.

◆ mTrackProjY

float WAIMapPoint::mTrackProjY = 0.0f

Definition at line 117 of file WAIMapPoint.h.

◆ mTrackViewCos

float WAIMapPoint::mTrackViewCos = 0.0f

Definition at line 122 of file WAIMapPoint.h.

◆ mWorldPos

cv::Mat WAIMapPoint::mWorldPos
protected

Definition at line 154 of file WAIMapPoint.h.

◆ nNextId

long unsigned int WAIMapPoint::nNextId = 0
static

Definition at line 110 of file WAIMapPoint.h.

◆ nObs

int WAIMapPoint::nObs = 0

Definition at line 112 of file WAIMapPoint.h.

◆ refKfSource

RefKfSource WAIMapPoint::refKfSource = RefKfSource_None

Definition at line 100 of file WAIMapPoint.h.


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