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((long)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  size_t N = vDescriptors.size();
345 
346  float** Distances = new float*[N];
347  for (size_t i = 0; i < N; ++i)
348  Distances[i] = new float[N];
349 
350  for (size_t i = 0; i < N; i++)
351  {
352  Distances[i][i] = 0;
353  for (size_t j = i + 1; j < N; j++)
354  {
355  int distij = ORBmatcher::DescriptorDistance(vDescriptors[i], vDescriptors[j]);
356  Distances[i][j] = (float)distij;
357  Distances[j][i] = (float)distij;
358  }
359  }
360 
361  // Take the descriptor with least median distance to the rest
362  int BestMedian = INT_MAX;
363  int BestIdx = 0;
364  for (size_t i = 0; i < N; i++)
365  {
366  vector<int> vDists(Distances[i], Distances[i] + N);
367  sort(vDists.begin(), vDists.end());
368  int median = vDists[(uint64_t)(0.5f * (N - 1))];
369 
370  if (median < BestMedian)
371  {
372  BestMedian = median;
373  BestIdx = (int)i;
374  }
375  }
376 
377  //free Distances
378  for (size_t i = 0; i < N; ++i)
379  delete Distances[i];
380  delete[] Distances;
381 
382  {
383  unique_lock<mutex> lock(mMutexFeatures);
384  mDescriptor = vDescriptors[BestIdx].clone();
385  }
386 }
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 388 of file WAIMapPoint.cpp.

389 {
390  unique_lock<mutex> lock(mMutexFeatures);
391  return mDescriptor.clone();
392 }

◆ 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) / (float)mnVisible;
311 }

◆ GetIndexInKeyFrame()

int WAIMapPoint::GetIndexInKeyFrame ( WAIKeyFrame pKF)

Definition at line 400 of file WAIMapPoint.cpp.

401 {
402  unique_lock<mutex> lock(mMutexFeatures);
403  if (mObservations.count(pKF))
404  return (int)mObservations[pKF];
405  else
406  return -1;
407 }

◆ GetMaxDistance()

float WAIMapPoint::GetMaxDistance ( )

Definition at line 476 of file WAIMapPoint.cpp.

477 {
478  return mfMaxDistance;
479 }

◆ GetMaxDistanceInvariance()

float WAIMapPoint::GetMaxDistanceInvariance ( )

Definition at line 465 of file WAIMapPoint.cpp.

466 {
467  unique_lock<mutex> lock(mMutexPos);
468  return 1.2f * mfMaxDistance;
469 }
std::mutex mMutexPos
Definition: WAIMapPoint.h:180

◆ GetMinDistance()

float WAIMapPoint::GetMinDistance ( )

Definition at line 471 of file WAIMapPoint.cpp.

472 {
473  return mfMinDistance;
474 }

◆ GetMinDistanceInvariance()

float WAIMapPoint::GetMinDistanceInvariance ( )

Definition at line 459 of file WAIMapPoint.cpp.

460 {
461  unique_lock<mutex> lock(mMutexPos);
462  return 0.8f * mfMinDistance;
463 }

◆ 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 537 of file WAIMapPoint.cpp.

538 {
539  size_t size = 0;
540 
541  size += sizeof(*this);
542  size += getSizeOfCvMat(mWorldPos);
543  size += getSizeOfCvMat(mNormalVector);
544  size += getSizeOfCvMat(mDescriptor);
545  size += getSizeOfCvMat(mPosGBA);
546 
547  return size;
548 }
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 525 of file WAIMapPoint.cpp.

526 {
527  size_t size = 0;
528  if (mat.isContinuous())
529  size = mat.total() * mat.elemSize();
530  else
531  {
532  size = mat.step[0] * mat.rows;
533  }
534  return size;
535 }

◆ 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 409 of file WAIMapPoint.cpp.

410 {
411  unique_lock<mutex> lock(mMutexFeatures);
412  return (mObservations.count(pKF));
413 }

◆ 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 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) / pF->mfLogScaleFactor);
517  if (nScale < 0)
518  nScale = 0;
519  else if (nScale >= pF->mnScaleLevels)
520  nScale = pF->mnScaleLevels - 1;
521 
522  return nScale;
523 }
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 491 of file WAIMapPoint.cpp.

492 {
493  float ratio;
494  {
495  unique_lock<mutex> lock(mMutexPos);
496  ratio = mfMaxDistance / currentDist;
497  }
498 
499  int nScale = (int)ceil(log(ratio) / pKF->mfLogScaleFactor);
500  if (nScale < 0)
501  nScale = 0;
502  else if (nScale >= pKF->mnScaleLevels)
503  nScale = pKF->mnScaleLevels - 1;
504 
505  return nScale;
506 }

◆ 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 394 of file WAIMapPoint.cpp.

395 {
396  unique_lock<mutex> lock(mMutexFeatures);
397  descriptor.copyTo(mDescriptor);
398 }

◆ SetMaxDistance()

void WAIMapPoint::SetMaxDistance ( float  maxDist)

Definition at line 481 of file WAIMapPoint.cpp.

482 {
483  mfMaxDistance = maxDist;
484 }

◆ SetMinDistance()

void WAIMapPoint::SetMinDistance ( float  minDist)

Definition at line 486 of file WAIMapPoint.cpp.

487 {
488  mfMinDistance = minDist;
489 }

◆ 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 416 of file WAIMapPoint.cpp.

417 {
418  map<WAIKeyFrame*, size_t> observations;
419  WAIKeyFrame* pRefKF;
420  cv::Mat Pos;
421  {
422  unique_lock<mutex> lock1(mMutexFeatures);
423  unique_lock<mutex> lock2(mMutexPos);
424  if (mbBad)
425  return;
426  observations = mObservations;
427  pRefKF = mpRefKF;
428  Pos = mWorldPos.clone();
429  }
430 
431  if (observations.empty())
432  return;
433 
434  cv::Mat normal = cv::Mat::zeros(3, 1, CV_32F);
435  int n = 0;
436  for (map<WAIKeyFrame*, size_t>::iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++)
437  {
438  WAIKeyFrame* pKF = mit->first;
439  cv::Mat Owi = pKF->GetCameraCenter();
440  cv::Mat normali = mWorldPos - Owi;
441  normal = normal + normali / cv::norm(normali);
442  n++;
443  }
444 
445  cv::Mat PC = Pos - pRefKF->GetCameraCenter();
446  const float dist = (float)cv::norm(PC);
447  const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;
448  const float levelScaleFactor = pRefKF->mvScaleFactors[level];
449  const int nLevels = pRefKF->mnScaleLevels;
450 
451  {
452  unique_lock<mutex> lock3(mMutexPos);
453  mfMaxDistance = dist * levelScaleFactor;
454  mfMinDistance = mfMaxDistance / pRefKF->mvScaleFactors[nLevels - 1];
455  mNormalVector = normal / n;
456  }
457 }
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: