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

#include <WAISlamTools.h>

Inheritance diagram for WAISlamTools:
[legend]

Static Public Member Functions

static void drawKeyPointInfo (WAIFrame &frame, cv::Mat &image, float scale)
 
static void drawKeyPointMatches (WAIFrame &frame, cv::Mat &image, float scale)
 
static void drawInitInfo (WAIInitializerData &iniData, WAIFrame &frame, cv::Mat &imageBGR, float scale)
 
static bool initialize (WAIInitializerData &iniData, WAIFrame &frame, WAIOrbVocabulary *voc, LocalMap &localMap, int mapPointsNeeded=100)
 
static bool genInitialMap (WAIMap *globalMap, LocalMapping *localMapper, LoopClosing *loopCloser, LocalMap &localMap)
 
static bool oldInitialize (WAIFrame &frame, WAIInitializerData &iniData, WAIMap *map, LocalMap &localMap, LocalMapping *localMapper, LoopClosing *loopCloser, WAIOrbVocabulary *voc, int mapPointsNeeded=100)
 
static int findFrameFixedMapMatches (WAIFrame &frame, WAIMap *waiMap, std::vector< cv::Point2f > &points2d, std::vector< cv::Point3f > &points3d)
 
static bool relocalization (WAIFrame &currentFrame, WAIMap *waiMap, LocalMap &localMap, float minCommonWordFactor, int &inliers, bool minAccScoreFilter=false)
 
static bool relocalizationGPS (WAIFrame &currentFrame, WAIMap *waiMap, LocalMap &localMap, cv::Mat locENU, cv::Mat dirENU, float minCommonWordFactor, int &inliers, bool minAccScoreFilter)
 
static bool tracking (WAIMap *map, LocalMap &localMap, WAIFrame &frame, WAIFrame &lastFrame, int lastRelocFrameId, cv::Mat &velocity, int &inliers)
 
static bool strictTracking (WAIMap *map, LocalMap &localMap, WAIFrame &frame, WAIFrame &lastFrame, int lastRelocFrameId, int &inliers)
 
static bool trackLocalMap (LocalMap &localMap, WAIFrame &frame, int lastRelocFrameId, int &inliers)
 
static void mapping (WAIMap *map, LocalMap &localMap, LocalMapping *localMapper, WAIFrame &frame, int inliers, const unsigned long lastRelocFrameId, unsigned long &lastKeyFrameFrameId)
 
static void strictMapping (WAIMap *map, LocalMap &localMap, LocalMapping *localMapper, WAIFrame &frame, int inliers, const unsigned long lastRelocFrameId, unsigned long &lastKeyFrameFrameId)
 
static void motionModel (WAIFrame &frame, WAIFrame &lastFrame, cv::Mat &velocity, cv::Mat &pose)
 
static bool trackReferenceKeyFrame (LocalMap &map, WAIFrame &lastFrame, WAIFrame &frame)
 
static bool strictTrackReferenceKeyFrame (LocalMap &map, WAIFrame &lastFrame, WAIFrame &frame)
 
static bool trackWithMotionModel (cv::Mat velocity, WAIFrame &previousFrame, WAIFrame &frame)
 
static void updateLocalMap (WAIFrame &frame, LocalMap &localMap)
 
static int trackLocalMapPoints (LocalMap &localMap, int lastRelocFrameId, WAIFrame &frame)
 
static bool needNewKeyFrame (WAIMap *globalMap, LocalMap &localMap, LocalMapping *localMapper, WAIFrame &frame, int nInliners, const unsigned long lastRelocFrameId, const unsigned long lastKeyFrameFrameId)
 
static bool strictNeedNewKeyFrame (WAIMap *map, LocalMap &localMap, LocalMapping *localMapper, WAIFrame &frame, int nInliers, const unsigned long lastRelocFrameId, const unsigned long lastKeyFrameFrameId)
 
static void createNewKeyFrame (LocalMapping *localMapper, LocalMap &localMap, WAIMap *globalMap, WAIFrame &frame, unsigned long &lastKeyFrameFrameId)
 
static WAIFrame createMarkerFrame (std::string markerFile, KPextractor *markerExtractor, const cv::Mat &markerCameraIntrinsic, WAIOrbVocabulary *voc)
 
static bool findMarkerHomography (WAIFrame &markerFrame, WAIKeyFrame *kfCand, cv::Mat &homography, int minMatches)
 
static bool doMarkerMapPreprocessing (std::string markerFile, cv::Mat &nodeTransform, float markerWidthInM, KPextractor *markerExtractor, WAIMap *map, const cv::Mat &markerCameraIntrinsic, WAIOrbVocabulary *voc)
 
static bool detectCycle (WAIKeyFrame *kf, std::set< WAIKeyFrame * > &visitedNode)
 
static bool checkKFConnectionsTree (WAIMap *map)
 

Protected Member Functions

virtual ~WAISlamTools ()
 
 WAISlamTools ()
 

Static Protected Member Functions

static void countReprojectionOutliers (WAIFrame &frame, unsigned int &m, unsigned int &n, unsigned int &outliers)
 

Protected Attributes

cv::Mat _distortion
 
cv::Mat _cameraIntrinsic
 
cv::Mat _cameraExtrinsic
 
cv::Mat _cameraExtrinsicGuess
 
WAIInitializerData _iniData
 
WAIFrame _lastFrame
 
std::unique_ptr< WAIMap_globalMap
 
LocalMap _localMap
 
WAIOrbVocabulary_voc = nullptr
 
cv::Mat _velocity
 
bool _initialized = false
 
LocalMapping * _localMapping = nullptr
 
LoopClosing * _loopClosing = nullptr
 
std::thread * _processNewKeyFrameThread = nullptr
 
std::vector< std::thread * > _mappingThreads
 
std::thread * _loopClosingThread = nullptr
 

Detailed Description

This class should not be instanced. It contains only pure virtual methods and some variables with getter that are useful for slam in a subclass.

Definition at line 40 of file WAISlamTools.h.

Constructor & Destructor Documentation

◆ ~WAISlamTools()

virtual WAISlamTools::~WAISlamTools ( )
inlineprotectedvirtual

Definition at line 192 of file WAISlamTools.h.

192 {}

◆ WAISlamTools()

WAISlamTools::WAISlamTools ( )
inlineprotected

Definition at line 193 of file WAISlamTools.h.

193 {};

Member Function Documentation

◆ checkKFConnectionsTree()

bool WAISlamTools::checkKFConnectionsTree ( WAIMap map)
static

Definition at line 1794 of file WAISlamTools.cpp.

1795 {
1796  std::vector<WAIKeyFrame*> allKfs = map->GetAllKeyFrames();
1797  unsigned int countGood = 0;
1798  WAIKeyFrame* root = nullptr;
1799  bool b = true;
1800 
1801  for (WAIKeyFrame* kf : allKfs)
1802  {
1803  if (kf->isBad())
1804  continue;
1805 
1806  countGood++;
1807 
1808  if (kf->mnId == 0)
1809  root = kf;
1810 
1811  if ((kf->GetParent() == nullptr || kf->GetParent()->isBad()) && kf->mnId != 0)
1812  {
1813  std::cout << "kf " << kf->mnId << " has no parent" << std::endl;
1814  b = false;
1815  }
1816  }
1817  std::set<WAIKeyFrame*> visitedNode;
1818 
1819  if (root == nullptr)
1820  {
1821  std::cout << "no root kf" << std::endl;
1822  }
1823  else
1824  {
1825  if (detectCycle(root, visitedNode))
1826  {
1827  std::cout << "keyframe graph has a cycle" << std::endl;
1828  b = false;
1829  }
1830  if (visitedNode.size() != countGood)
1831  {
1832  std::cout << "keyframe graph is split" << std::endl;
1833  std::cout << "visitedNode.size() = " << visitedNode.size() << " != number of good keyframe = " << countGood << std::endl;
1834  b = false;
1835  }
1836  }
1837  return b;
1838 }
AR Keyframe node class.
Definition: WAIKeyFrame.h:60
long unsigned int mnId
Definition: WAIKeyFrame.h:175
std::vector< WAIKeyFrame * > GetAllKeyFrames()
Definition: WAIMap.cpp:104
static bool detectCycle(WAIKeyFrame *kf, std::set< WAIKeyFrame * > &visitedNode)

◆ countReprojectionOutliers()

void WAISlamTools::countReprojectionOutliers ( WAIFrame frame,
unsigned int &  m,
unsigned int &  n,
unsigned int &  outliers 
)
staticprotected

Definition at line 574 of file WAISlamTools.cpp.

575 {
576  //calculation of mean reprojection error
577  double reprojectionError = 0.0;
578 
579  //current frame extrinsic
580  const cv::Mat Rcw = frame.GetRotationCW();
581  const cv::Mat tcw = frame.GetTranslationCW();
582  //current frame intrinsics
583  const float fx = frame.fx;
584  const float fy = frame.fy;
585  const float cx = frame.cx;
586  const float cy = frame.cy;
587  n = 0;
588  m = 0;
589  outliers = 0;
590 
591  for (size_t i = 0; i < frame.N; i++)
592  {
593  if (frame.mvpMapPoints[i] == nullptr || frame.mvpMapPoints[i]->isBad())
594  continue;
595 
596  if (!frame.mvpMapPoints[i]->loadedFromMap())
597  {
598  m++;
599  continue;
600  }
601 
602  n++;
603 
604  // 3D in absolute coordinates
605  cv::Mat Pw = frame.mvpMapPoints[i]->GetWorldPos();
606  // 3D in camera coordinates
607  const cv::Mat Pc = Rcw * Pw + tcw;
608  const float& PcX = Pc.at<float>(0);
609  const float& PcY = Pc.at<float>(1);
610  const float& PcZ = Pc.at<float>(2);
611 
612  // Check positive depth
613  if (PcZ < 0.0f)
614  continue;
615 
616  // Project in image and check it is not outside
617  const float invz = 1.0f / PcZ;
618  const float u = fx * PcX * invz + cx;
619  const float v = fy * PcY * invz + cy;
620 
621  cv::Point2f ptProj(u, v);
622  //Use distorted points because we have to undistort the image later
623  const auto& ptImg = frame.mvKeysUn[i].pt;
624 
625  if (cv::norm(cv::Mat(ptImg), cv::Mat(ptProj)) > 1.3)
626  outliers++;
627  }
628 }
static float cx
Definition: WAIFrame.h:116
static float fx
Definition: WAIFrame.h:114
static float fy
Definition: WAIFrame.h:115
static float cy
Definition: WAIFrame.h:117
std::vector< WAIMapPoint * > mvpMapPoints
Definition: WAIFrame.h:147
cv::Mat GetRotationCW()
Definition: WAIFrame.h:88
int N
Definition: WAIFrame.h:123
cv::Mat GetTranslationCW()
Definition: WAIFrame.h:82
std::vector< cv::KeyPoint > mvKeysUn
Definition: WAIFrame.h:129

◆ createMarkerFrame()

WAIFrame WAISlamTools::createMarkerFrame ( std::string  markerFile,
KPextractor *  markerExtractor,
const cv::Mat &  markerCameraIntrinsic,
WAIOrbVocabulary voc 
)
static

Definition at line 1401 of file WAISlamTools.cpp.

1405 {
1406  cv::Mat markerImgGray = cv::imread(markerFile, cv::IMREAD_GRAYSCALE);
1407 
1408  float fyCam = (float)markerCameraIntrinsic.at<double>(1, 1);
1409  float cyCam = (float)markerCameraIntrinsic.at<double>(1, 2);
1410  float fov = (float)(2.0f * atan2(cyCam, fyCam) * 180.0f / M_PI);
1411 
1412  float cx = (float)markerImgGray.cols * 0.5f;
1413  float cy = (float)markerImgGray.rows * 0.5f;
1414  float fy = cy / tanf(fov * 0.5f * (float)M_PI / 180.0f);
1415  float fx = fy;
1416 
1417  // TODO(dgj1): pass actual calibration for marker frame?
1418  cv::Mat markerCameraMat = (cv::Mat_<float>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
1419  cv::Mat markerDistortionMat = cv::Mat::zeros(4, 1, CV_32F);
1420 
1421  WAIFrame result = WAIFrame(markerImgGray, 0.0f, markerExtractor, markerCameraMat, markerDistortionMat, voc, true);
1422  result = WAIFrame(markerImgGray, 0.0f, markerExtractor, markerCameraMat, markerDistortionMat, voc, true);
1423  result = WAIFrame(markerImgGray, 0.0f, markerExtractor, markerCameraMat, markerDistortionMat, voc, true);
1424  return result;
1425 }

◆ createNewKeyFrame()

void WAISlamTools::createNewKeyFrame ( LocalMapping *  localMapper,
LocalMap &  localMap,
WAIMap globalMap,
WAIFrame frame,
unsigned long &  lastKeyFrameFrameId 
)
static

Definition at line 559 of file WAISlamTools.cpp.

564 {
565  WAIKeyFrame* pKF = new WAIKeyFrame(frame);
566 
567  lastKeyFrameFrameId = frame.mnId;
568  lmap.refKF = pKF;
569  frame.mpReferenceKF = pKF;
570 
571  localMapper->InsertKeyFrame(pKF);
572 }
long unsigned int mnId
Definition: WAIFrame.h:159
WAIKeyFrame * mpReferenceKF
Definition: WAIFrame.h:165

◆ detectCycle()

bool WAISlamTools::detectCycle ( WAIKeyFrame kf,
std::set< WAIKeyFrame * > &  visitedNode 
)
static

Definition at line 1768 of file WAISlamTools.cpp.

1769 {
1770  std::stack<WAIKeyFrame*> nodeStack;
1771 
1772  nodeStack.push(kf);
1773 
1774  while (!nodeStack.empty())
1775  {
1776  WAIKeyFrame* k = nodeStack.top();
1777  nodeStack.pop();
1778 
1779  if (visitedNode.count(k) == 0)
1780  {
1781  visitedNode.insert(k);
1782  for (WAIKeyFrame* c : k->GetChilds())
1783  {
1784  if (!c->isBad())
1785  nodeStack.push(c);
1786  }
1787  }
1788  else
1789  return true;
1790  }
1791  return false;
1792 }
std::set< WAIKeyFrame * > GetChilds()

◆ doMarkerMapPreprocessing()

bool WAISlamTools::doMarkerMapPreprocessing ( std::string  markerFile,
cv::Mat &  nodeTransform,
float  markerWidthInM,
KPextractor *  markerExtractor,
WAIMap map,
const cv::Mat &  markerCameraIntrinsic,
WAIOrbVocabulary voc 
)
static

Definition at line 1467 of file WAISlamTools.cpp.

1474 {
1475  // Additional steps to save marker map
1476  // 1. Find matches to marker on two keyframes
1477  // 1.a Extract features from marker image
1478  WAIFrame markerFrame = createMarkerFrame(markerFile,
1479  markerExtractor,
1480  markerCameraIntrinsic,
1481  voc);
1482 
1483  // 1.b Find keyframes with enough matches to marker image
1484  std::vector<WAIKeyFrame*> kfs = map->GetAllKeyFrames();
1485 
1486  WAIKeyFrame* matchedKf1 = nullptr;
1487  WAIKeyFrame* matchedKf2 = nullptr;
1488 
1489  cv::Mat ul = cv::Mat(cv::Point3f(0, 0, 1));
1490  cv::Mat ur = cv::Mat(cv::Point3f((float)markerFrame.imgGray.cols, 0, 1));
1491  cv::Mat ll = cv::Mat(cv::Point3f(0, (float)markerFrame.imgGray.rows, 1));
1492  cv::Mat lr = cv::Mat(cv::Point3f((float)markerFrame.imgGray.cols, (float)markerFrame.imgGray.rows, 1));
1493 
1494  cv::Mat ulKf1, urKf1, llKf1, lrKf1, ulKf2, urKf2, llKf2, lrKf2;
1495  cv::Mat ul3D, ur3D, ll3D, lr3D;
1496  cv::Mat AC, AB, n;
1497 
1498  for (int i1 = 0; i1 < kfs.size() - 1; i1++)
1499  {
1500  WAIKeyFrame* kfCand1 = kfs[i1];
1501 
1502  if (kfCand1->isBad()) continue;
1503 
1504  // 2. Calculate homography between the keyframes and marker
1505  cv::Mat homography1;
1506  if (findMarkerHomography(markerFrame, kfCand1, homography1, 50))
1507  {
1508  // 3.a Calculate position of the markers cornerpoints on first keyframe in 2D
1509  // NOTE(dgj1): assumption that intrinsic camera parameters are the same
1510  // TODO(dgj1): think about this assumption
1511  ulKf1 = homography1 * ul;
1512  ulKf1 /= ulKf1.at<float>(2, 0);
1513  urKf1 = homography1 * ur;
1514  urKf1 /= urKf1.at<float>(2, 0);
1515  llKf1 = homography1 * ll;
1516  llKf1 /= llKf1.at<float>(2, 0);
1517  lrKf1 = homography1 * lr;
1518  lrKf1 /= lrKf1.at<float>(2, 0);
1519 
1520  for (int i2 = i1 + 1; i2 < kfs.size(); i2++)
1521  {
1522  WAIKeyFrame* kfCand2 = kfs[i2];
1523 
1524  if (kfCand2->isBad()) continue;
1525 
1526  cv::Mat homography2;
1527  if (findMarkerHomography(markerFrame, kfCand2, homography2, 50))
1528  {
1529  // 3.b Calculate position of the markers cornerpoints on second keyframe in 2D
1530  // NOTE(dgj1): assumption that intrinsic camera parameters are the same
1531  // TODO(dgj1): think about this assumption
1532  ulKf2 = homography2 * ul;
1533  ulKf2 /= ulKf2.at<float>(2, 0);
1534  urKf2 = homography2 * ur;
1535  urKf2 /= urKf2.at<float>(2, 0);
1536  llKf2 = homography2 * ll;
1537  llKf2 /= llKf2.at<float>(2, 0);
1538  lrKf2 = homography2 * lr;
1539  lrKf2 /= lrKf2.at<float>(2, 0);
1540 
1541  // 4. Triangulate position of the markers cornerpoints
1542  cv::Mat Rcw1 = kfCand1->GetRotation();
1543  cv::Mat Rwc1 = Rcw1.t();
1544  cv::Mat tcw1 = kfCand1->GetTranslation();
1545  cv::Mat Tcw1(3, 4, CV_32F);
1546  Rcw1.copyTo(Tcw1.colRange(0, 3));
1547  tcw1.copyTo(Tcw1.col(3));
1548 
1549  const float& fx1 = kfCand1->fx;
1550  const float& fy1 = kfCand1->fy;
1551  const float& cx1 = kfCand1->cx;
1552  const float& cy1 = kfCand1->cy;
1553  const float& invfx1 = kfCand1->invfx;
1554  const float& invfy1 = kfCand1->invfy;
1555 
1556  cv::Mat Rcw2 = kfCand2->GetRotation();
1557  cv::Mat Rwc2 = Rcw2.t();
1558  cv::Mat tcw2 = kfCand2->GetTranslation();
1559  cv::Mat Tcw2(3, 4, CV_32F);
1560  Rcw2.copyTo(Tcw2.colRange(0, 3));
1561  tcw2.copyTo(Tcw2.col(3));
1562 
1563  const float& fx2 = kfCand2->fx;
1564  const float& fy2 = kfCand2->fy;
1565  const float& cx2 = kfCand2->cx;
1566  const float& cy2 = kfCand2->cy;
1567  const float& invfx2 = kfCand2->invfx;
1568  const float& invfy2 = kfCand2->invfy;
1569 
1570  {
1571  cv::Mat ul1 = (cv::Mat_<float>(3, 1) << (ulKf1.at<float>(0, 0) - cx1) * invfx1, (ulKf1.at<float>(1, 0) - cy1) * invfy1, 1.0);
1572  cv::Mat ul2 = (cv::Mat_<float>(3, 1) << (ulKf2.at<float>(0, 0) - cx2) * invfx2, (ulKf2.at<float>(1, 0) - cy2) * invfy2, 1.0);
1573 
1574  // Linear Triangulation Method
1575  cv::Mat A(4, 4, CV_32F);
1576  A.row(0) = ul1.at<float>(0) * Tcw1.row(2) - Tcw1.row(0);
1577  A.row(1) = ul1.at<float>(1) * Tcw1.row(2) - Tcw1.row(1);
1578  A.row(2) = ul2.at<float>(0) * Tcw2.row(2) - Tcw2.row(0);
1579  A.row(3) = ul2.at<float>(1) * Tcw2.row(2) - Tcw2.row(1);
1580 
1581  cv::Mat w, u, vt;
1582  cv::SVD::compute(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
1583 
1584  ul3D = vt.row(3).t();
1585 
1586  if (ul3D.at<float>(3) != 0)
1587  {
1588  // Euclidean coordinates
1589  ul3D = ul3D.rowRange(0, 3) / ul3D.at<float>(3);
1590  }
1591  }
1592 
1593  {
1594  cv::Mat ur1 = (cv::Mat_<float>(3, 1) << (urKf1.at<float>(0, 0) - cx1) * invfx1, (urKf1.at<float>(1, 0) - cy1) * invfy1, 1.0);
1595  cv::Mat ur2 = (cv::Mat_<float>(3, 1) << (urKf2.at<float>(0, 0) - cx2) * invfx2, (urKf2.at<float>(1, 0) - cy2) * invfy2, 1.0);
1596 
1597  // Linear Triangulation Method
1598  cv::Mat A(4, 4, CV_32F);
1599  A.row(0) = ur1.at<float>(0) * Tcw1.row(2) - Tcw1.row(0);
1600  A.row(1) = ur1.at<float>(1) * Tcw1.row(2) - Tcw1.row(1);
1601  A.row(2) = ur2.at<float>(0) * Tcw2.row(2) - Tcw2.row(0);
1602  A.row(3) = ur2.at<float>(1) * Tcw2.row(2) - Tcw2.row(1);
1603 
1604  cv::Mat w, u, vt;
1605  cv::SVD::compute(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
1606 
1607  ur3D = vt.row(3).t();
1608 
1609  if (ur3D.at<float>(3) != 0)
1610  {
1611  // Euclidean coordinates
1612  ur3D = ur3D.rowRange(0, 3) / ur3D.at<float>(3);
1613  }
1614  }
1615 
1616  {
1617  cv::Mat ll1 = (cv::Mat_<float>(3, 1) << (llKf1.at<float>(0, 0) - cx1) * invfx1, (llKf1.at<float>(1, 0) - cy1) * invfy1, 1.0);
1618  cv::Mat ll2 = (cv::Mat_<float>(3, 1) << (llKf2.at<float>(0, 0) - cx2) * invfx2, (llKf2.at<float>(1, 0) - cy2) * invfy2, 1.0);
1619 
1620  // Linear Triangulation Method
1621  cv::Mat A(4, 4, CV_32F);
1622  A.row(0) = ll1.at<float>(0) * Tcw1.row(2) - Tcw1.row(0);
1623  A.row(1) = ll1.at<float>(1) * Tcw1.row(2) - Tcw1.row(1);
1624  A.row(2) = ll2.at<float>(0) * Tcw2.row(2) - Tcw2.row(0);
1625  A.row(3) = ll2.at<float>(1) * Tcw2.row(2) - Tcw2.row(1);
1626 
1627  cv::Mat w, u, vt;
1628  cv::SVD::compute(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
1629 
1630  ll3D = vt.row(3).t();
1631 
1632  if (ll3D.at<float>(3) != 0)
1633  {
1634  // Euclidean coordinates
1635  ll3D = ll3D.rowRange(0, 3) / ll3D.at<float>(3);
1636  }
1637  }
1638 
1639  {
1640  cv::Mat lr1 = (cv::Mat_<float>(3, 1) << (lrKf1.at<float>(0, 0) - cx1) * invfx1, (lrKf1.at<float>(1, 0) - cy1) * invfy1, 1.0);
1641  cv::Mat lr2 = (cv::Mat_<float>(3, 1) << (lrKf2.at<float>(0, 0) - cx2) * invfx2, (lrKf2.at<float>(1, 0) - cy2) * invfy2, 1.0);
1642 
1643  // Linear Triangulation Method
1644  cv::Mat A(4, 4, CV_32F);
1645  A.row(0) = lr1.at<float>(0) * Tcw1.row(2) - Tcw1.row(0);
1646  A.row(1) = lr1.at<float>(1) * Tcw1.row(2) - Tcw1.row(1);
1647  A.row(2) = lr2.at<float>(0) * Tcw2.row(2) - Tcw2.row(0);
1648  A.row(3) = lr2.at<float>(1) * Tcw2.row(2) - Tcw2.row(1);
1649 
1650  cv::Mat w, u, vt;
1651  cv::SVD::compute(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
1652 
1653  lr3D = vt.row(3).t();
1654 
1655  if (lr3D.at<float>(3) != 0)
1656  {
1657  // Euclidean coordinates
1658  lr3D = lr3D.rowRange(0, 3) / lr3D.at<float>(3);
1659  }
1660  }
1661 
1662  AC = ll3D - ul3D;
1663  AB = ur3D - ul3D;
1664 
1665  cv::Vec3f vAC = AC;
1666  cv::Vec3f vAB = AB;
1667 
1668  cv::Vec3f vn = vAB.cross(vAC);
1669  n = cv::Mat(vn);
1670 
1671  cv::Mat AD = lr3D - ul3D;
1672  cv::Vec3f vAD = AD;
1673 
1674  float d = (float)cv::norm(vn.dot(vAD)) / (float)cv::norm(vn);
1675  if (d < 0.01f)
1676  {
1677  matchedKf1 = kfCand1;
1678  matchedKf2 = kfCand2;
1679 
1680  break;
1681  }
1682  }
1683  }
1684  }
1685 
1686  if (matchedKf2) break;
1687  }
1688 
1689  if (!matchedKf1 || !matchedKf2)
1690  {
1691  return false;
1692  }
1693 
1694  // 5. Cull mappoints outside of marker
1695  std::vector<WAIMapPoint*> mapPoints = map->GetAllMapPoints();
1696 
1697  cv::Mat system = cv::Mat::zeros(3, 3, CV_32F);
1698  AC.copyTo(system.rowRange(0, 3).col(0));
1699  AB.copyTo(system.rowRange(0, 3).col(1));
1700  n.copyTo(system.rowRange(0, 3).col(2));
1701 
1702  cv::Mat systemInv = system.inv();
1703 
1704  for (int i = 0; i < mapPoints.size(); i++)
1705  {
1706  WAIMapPoint* mp = mapPoints[i];
1707 
1708  if (mp->isBad()) continue;
1709 
1710  cv::Mat sol = systemInv * (mp->GetWorldPos() - ul3D);
1711 
1712  if (sol.at<float>(0, 0) < 0 || sol.at<float>(0, 0) > 1 ||
1713  sol.at<float>(1, 0) < 0 || sol.at<float>(1, 0) > 1 ||
1714  sol.at<float>(2, 0) < -0.1f || sol.at<float>(2, 0) > 0.1f)
1715  {
1716  mp->SetBadFlag();
1717  }
1718  }
1719 
1720  for (int i = 0; i < kfs.size(); i++)
1721  {
1722  WAIKeyFrame* kf = kfs[i];
1723 
1724  if (kf->mnId == 0 || kf->isBad()) continue;
1725 
1726  int mpCount = 0;
1727 
1728  std::vector<WAIMapPoint*> mps = kf->GetMapPointMatches();
1729  for (int j = 0; j < mps.size(); j++)
1730  {
1731  WAIMapPoint* mp = mps[j];
1732 
1733  if (!mp || mp->isBad()) continue;
1734 
1735  mpCount++;
1736  }
1737 
1738  if (mpCount <= 0)
1739  {
1740  kf->SetBadFlag();
1741  }
1742  }
1743 
1744  cv::Mat systemNorm = cv::Mat::zeros(3, 3, CV_32F);
1745  systemNorm.rowRange(0, 3).col(0) = system.rowRange(0, 3).col(1) / cv::norm(AB);
1746  systemNorm.rowRange(0, 3).col(1) = system.rowRange(0, 3).col(0) / cv::norm(AC);
1747  systemNorm.rowRange(0, 3).col(2) = system.rowRange(0, 3).col(2) / cv::norm(n);
1748 
1749  cv::Mat systemNormInv = systemNorm.inv();
1750 
1751  nodeTransform = cv::Mat::eye(4, 4, CV_32F);
1752  cv::Mat ul3Dinv = -systemNormInv * ul3D;
1753  ul3Dinv.copyTo(nodeTransform.rowRange(0, 3).col(3));
1754  systemNormInv.copyTo(nodeTransform.rowRange(0, 3).colRange(0, 3));
1755 
1756  cv::Mat scaleMat = cv::Mat::eye(4, 4, CV_32F);
1757  float markerWidthInRef = (float)cv::norm(ul3D - ur3D);
1758  float scaleFactor = markerWidthInM / markerWidthInRef;
1759  scaleMat.at<float>(0, 0) = scaleFactor;
1760  scaleMat.at<float>(1, 1) = scaleFactor;
1761  scaleMat.at<float>(2, 2) = scaleFactor;
1762 
1763  nodeTransform = scaleMat * nodeTransform;
1764 
1765  return true;
1766 }
cv::Mat imgGray
Definition: WAIFrame.h:185
const float cy
Definition: WAIKeyFrame.h:214
void SetBadFlag()
cv::Mat GetTranslation()
const float fx
Definition: WAIKeyFrame.h:214
const float fy
Definition: WAIKeyFrame.h:214
const float cx
Definition: WAIKeyFrame.h:214
cv::Mat GetRotation()
const float invfy
Definition: WAIKeyFrame.h:214
const float invfx
Definition: WAIKeyFrame.h:214
std::vector< WAIMapPoint * > GetMapPointMatches()
std::vector< WAIMapPoint * > GetAllMapPoints()
Definition: WAIMap.cpp:110
void SetBadFlag()
cv::Mat GetWorldPos()
static WAIFrame createMarkerFrame(std::string markerFile, KPextractor *markerExtractor, const cv::Mat &markerCameraIntrinsic, WAIOrbVocabulary *voc)
static bool findMarkerHomography(WAIFrame &markerFrame, WAIKeyFrame *kfCand, cv::Mat &homography, int minMatches)

◆ drawInitInfo()

void WAISlamTools::drawInitInfo ( WAIInitializerData iniData,
WAIFrame frame,
cv::Mat &  imageBGR,
float  scale 
)
static

Definition at line 52 of file WAISlamTools.cpp.

53 {
54  //half rectangle width and rectangle width (values are estimated on 640x480)
55  int rhw = (int)(scale * 3.0f);
56  int rw = 2 * rhw + 1;
57 
58  for (unsigned int i = 0; i < iniData.initialFrame.mvKeys.size(); i++)
59  {
60  auto pt = scale * iniData.initialFrame.mvKeys[i].pt;
61  cv::rectangle(imageBGR,
62  cv::Rect((int)(pt.x - rhw), (int)(pt.y - rhw), rw, rw),
63  cv::Scalar(0, 0, 255));
64  }
65 
66  for (unsigned int i = 0; i < iniData.iniMatches.size(); i++)
67  {
68  if (iniData.iniMatches[i] >= 0)
69  {
70  cv::line(imageBGR,
71  scale * iniData.initialFrame.mvKeys[i].pt,
72  scale * newFrame.mvKeys[iniData.iniMatches[i]].pt,
73  cv::Scalar(0, 255, 0));
74  }
75  }
76 }
std::vector< cv::KeyPoint > mvKeys
Definition: WAIFrame.h:128
std::vector< int > iniMatches
Definition: WAISlamTools.h:23
WAIFrame initialFrame
Definition: WAISlamTools.h:20

◆ drawKeyPointInfo()

void WAISlamTools::drawKeyPointInfo ( WAIFrame frame,
cv::Mat &  image,
float  scale 
)
static

Definition at line 14 of file WAISlamTools.cpp.

15 {
16  //half rectangle width and rectangle width (values are estimated on 640x480)
17  int rhw = (int)(scale * 3.0f);
18  int rw = 2 * rhw + 1;
19  //show rectangle for all keypoints in current image
20  for (size_t i = 0; i < frame.N; i++)
21  {
22  //Use distorted points because we have to undistort the image later
23  //const auto& pt = mCurrentFrame.mvKeys[i].pt;
24  auto pt = scale * frame.mvKeys[i].pt;
25  cv::rectangle(image,
26  cv::Rect((int)(pt.x - rhw), (int)(pt.y - rhw), rw, rw),
27  cv::Scalar(0, 0, 255));
28  }
29 }

◆ drawKeyPointMatches()

void WAISlamTools::drawKeyPointMatches ( WAIFrame frame,
cv::Mat &  image,
float  scale 
)
static

Definition at line 31 of file WAISlamTools.cpp.

32 {
33  //half rectangle width and rectangle width (values are estimated on 640x480)
34  int rhw = (int)(scale * 3.0f);
35  int rw = 2 * rhw + 1;
36  for (size_t i = 0; i < frame.N; i++)
37  {
38  if (frame.mvpMapPoints[i])
39  {
40  if (frame.mvpMapPoints[i]->Observations() > 0)
41  {
42  //Use distorted points because we have to undistort the image later
43  auto pt = scale * frame.mvKeys[i].pt;
44  cv::rectangle(image,
45  cv::Rect((int)(pt.x - rhw), (int)(pt.y - rhw), rw, rw),
46  cv::Scalar(0, 255, 0));
47  }
48  }
49  }
50 }

◆ findFrameFixedMapMatches()

static int WAISlamTools::findFrameFixedMapMatches ( WAIFrame frame,
WAIMap waiMap,
std::vector< cv::Point2f > &  points2d,
std::vector< cv::Point3f > &  points3d 
)
static

◆ findMarkerHomography()

bool WAISlamTools::findMarkerHomography ( WAIFrame markerFrame,
WAIKeyFrame kfCand,
cv::Mat &  homography,
int  minMatches 
)
static

Definition at line 1427 of file WAISlamTools.cpp.

1431 {
1432  bool result = false;
1433 
1434  ORBmatcher matcher(0.9f, true);
1435 
1436  std::vector<int> markerMatchesToCurrentFrame;
1437  int nmatches = matcher.SearchForMarkerMap(markerFrame, *kfCand, markerMatchesToCurrentFrame);
1438 
1439  if (nmatches > minMatches)
1440  {
1441  std::vector<cv::Point2f> markerPoints;
1442  std::vector<cv::Point2f> framePoints;
1443 
1444  for (int j = 0; j < markerMatchesToCurrentFrame.size(); j++)
1445  {
1446  if (markerMatchesToCurrentFrame[j] >= 0)
1447  {
1448  markerPoints.push_back(markerFrame.mvKeysUn[j].pt);
1449  framePoints.push_back(kfCand->mvKeysUn[markerMatchesToCurrentFrame[j]].pt);
1450  }
1451  }
1452 
1453  homography = cv::findHomography(markerPoints,
1454  framePoints,
1455  cv::RANSAC);
1456 
1457  if (!homography.empty())
1458  {
1459  homography.convertTo(homography, CV_32F);
1460  result = true;
1461  }
1462  }
1463 
1464  return result;
1465 }
const std::vector< cv::KeyPoint > mvKeysUn
Definition: WAIKeyFrame.h:220

◆ genInitialMap()

bool WAISlamTools::genInitialMap ( WAIMap globalMap,
LocalMapping *  localMapper,
LoopClosing *  loopCloser,
LocalMap &  localMap 
)
static

Definition at line 238 of file WAISlamTools.cpp.

242 {
243  if (localMap.keyFrames.size() != 2)
244  return false;
245 
246  std::unique_lock<std::mutex> lock(map->mMutexMapUpdate, std::defer_lock);
247  lock.lock();
248  // Insert KFs in the map
249  map->AddKeyFrame(localMap.keyFrames[0]);
250  map->AddKeyFrame(localMap.keyFrames[1]);
251 
252  //Add to Map
253  for (size_t i = 0; i < localMap.mapPoints.size(); i++)
254  {
255  WAIMapPoint* pMP = localMap.mapPoints[i];
256  if (pMP)
257  map->AddMapPoint(pMP);
258  }
259 
260  // Bundle Adjustment
261  Optimizer::GlobalBundleAdjustemnt(map, 20);
262 
263  map->SetReferenceMapPoints(localMap.mapPoints);
264  map->mvpKeyFrameOrigins.push_back(localMap.keyFrames[0]);
265 
266  return true;
267 }

◆ initialize()

bool WAISlamTools::initialize ( WAIInitializerData iniData,
WAIFrame frame,
WAIOrbVocabulary voc,
LocalMap &  localMap,
int  mapPointsNeeded = 100 
)
static

Definition at line 78 of file WAISlamTools.cpp.

83 {
84  int matchesNeeded = mapPointsNeeded;
85 
86  if (!iniData.initializer)
87  {
88  // Set Reference Frame
89  if (frame.mvKeys.size() > matchesNeeded)
90  {
91  iniData.initialFrame = WAIFrame(frame);
92  iniData.prevMatched.resize(frame.mvKeysUn.size());
93  //ghm1: we store the undistorted keypoints of the initial frame in an extra vector
94  //todo: why not using mInitialFrame.mvKeysUn????
95  for (size_t i = 0; i < frame.mvKeysUn.size(); i++)
96  iniData.prevMatched[i] = frame.mvKeysUn[i].pt;
97 
98  iniData.initializer = new ORB_SLAM2::Initializer(frame, 1.0, 200);
99  //ghm1: clear mvIniMatches. it contains the index of the matched keypoint in the current frame
100  fill(iniData.iniMatches.begin(), iniData.iniMatches.end(), -1);
101  }
102  return false;
103  }
104 
105  // Try to initialize
106  if ((int)frame.mvKeys.size() <= matchesNeeded)
107  {
108  delete iniData.initializer;
109  iniData.initializer = static_cast<Initializer*>(NULL);
110  fill(iniData.iniMatches.begin(), iniData.iniMatches.end(), -1);
111  return false;
112  }
113 
114  // Find correspondences
115  ORBmatcher matcher(0.9f, true);
116  int nmatches = matcher.SearchForInitialization(iniData.initialFrame, frame, iniData.prevMatched, iniData.iniMatches, 100);
117 
118  // Check if there are enough correspondences
119  if (nmatches < matchesNeeded)
120  {
121  delete iniData.initializer;
122  iniData.initializer = static_cast<Initializer*>(NULL);
123  fill(iniData.iniMatches.begin(), iniData.iniMatches.end(), -1);
124  return false;
125  }
126 
127  cv::Mat Rcw; // Current Camera Rotation
128  cv::Mat tcw; // Current Camera Translation
129  vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
130 
131  if (iniData.initializer->Initialize(frame, iniData.iniMatches, Rcw, tcw, iniData.iniPoint3D, vbTriangulated))
132  {
133  for (size_t i = 0, iend = iniData.iniMatches.size(); i < iend; i++)
134  {
135  if (iniData.iniMatches[i] >= 0 && !vbTriangulated[i])
136  {
137  iniData.iniMatches[i] = -1;
138  nmatches--;
139  }
140  }
141  }
142  else
143  {
144  //delete iniData.initializer;
145  //iniData.initializer = static_cast<Initializer*>(NULL);
146  return false;
147  }
148 
149  // Set Frame Poses
150  iniData.initialFrame.SetPose(cv::Mat::eye(4, 4, CV_32F));
151  cv::Mat Tcw = cv::Mat::eye(4, 4, CV_32F);
152  Rcw.copyTo(Tcw.rowRange(0, 3).colRange(0, 3));
153  tcw.copyTo(Tcw.rowRange(0, 3).col(3));
154  frame.SetPose(Tcw);
155 
156  //ghm1: reset nNextId to 0! This is important otherwise the first keyframe cannot be identified via its id and a lot of stuff gets messed up!
157  //One problem we identified is in UpdateConnections: the first one is not allowed to have a parent,
158  //because the second will set the first as a parent too. We get problems later during culling.
159  //This also fixes a problem in first GlobalBundleAdjustment which messed up the map after a reset.
161 
162  // Create KeyFrames
163  WAIKeyFrame* pKFini = new WAIKeyFrame(iniData.initialFrame);
164  WAIKeyFrame* pKFcur = new WAIKeyFrame(frame);
165 
166  pKFini->ComputeBoW(voc);
167  pKFcur->ComputeBoW(voc);
168 
169  // Create MapPoints and associate to keyframes
170  for (size_t i = 0; i < iniData.iniMatches.size(); i++)
171  {
172  if (iniData.iniMatches[i] < 0)
173  continue;
174 
175  //Create MapPoint.
176  cv::Mat worldPos(iniData.iniPoint3D[i]);
177 
178  WAIMapPoint* pMP = new WAIMapPoint(worldPos, pKFcur);
179 
180  pKFini->AddMapPoint(pMP, i);
181  pKFcur->AddMapPoint(pMP, iniData.iniMatches[i]);
182 
183  pMP->AddObservation(pKFini, i);
184  pMP->AddObservation(pKFcur, iniData.iniMatches[i]);
185 
187  pMP->UpdateNormalAndDepth();
188 
189  //Fill Current Frame structure
190  frame.mvpMapPoints[iniData.iniMatches[i]] = pMP;
191  }
192 
193  // Update Connections
194  pKFini->FindAndUpdateConnections();
195  pKFcur->FindAndUpdateConnections();
196 
197  // Set median depth to 1
198  float medianDepth = pKFini->ComputeSceneMedianDepth(2);
199  float invMedianDepth = 1.0f / medianDepth;
200 
201  if (medianDepth < 0 || pKFcur->TrackedMapPoints(1) < mapPointsNeeded)
202  {
203  Utils::log("WAI", "Wrong initialization, reseting...");
205  WAIFrame::nNextId = 0;
208 
209  return false;
210  }
211 
212  localMap.keyFrames.push_back(pKFini);
213  localMap.keyFrames.push_back(pKFcur);
214  // Scale initial baseline
215  cv::Mat Tc2w = pKFcur->GetPose();
216  Tc2w.col(3).rowRange(0, 3) = Tc2w.col(3).rowRange(0, 3) * invMedianDepth;
217  pKFcur->SetPose(Tc2w);
218 
219  // Scale points
220  vector<WAIMapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches();
221  for (size_t iMP = 0; iMP < vpAllMapPoints.size(); iMP++)
222  {
223  if (vpAllMapPoints[iMP])
224  {
225  WAIMapPoint* pMP = vpAllMapPoints[iMP];
226  pMP->SetWorldPos(pMP->GetWorldPos() * invMedianDepth);
227  localMap.mapPoints.push_back(pMP);
228  }
229  }
230 
231  frame.SetPose(pKFcur->GetPose());
232  localMap.refKF = pKFcur;
233  frame.mpReferenceKF = pKFcur;
234 
235  return true;
236 }
static long unsigned int nNextId
Definition: WAIFrame.h:158
void SetPose(cv::Mat Tcw)
Definition: WAIFrame.cpp:178
static bool mbInitialComputations
Definition: WAIFrame.h:182
float ComputeSceneMedianDepth(const int q)
void SetPose(const cv::Mat &Tcw)
void AddMapPoint(WAIMapPoint *pMP, size_t idx)
void FindAndUpdateConnections(bool buildSpanningTree=true)
cv::Mat GetPose()
void ComputeBoW(WAIOrbVocabulary *vocabulary)
static long unsigned int nNextId
Definition: WAIKeyFrame.h:174
void AddObservation(WAIKeyFrame *pKF, size_t idx)
static long unsigned int nNextId
Definition: WAIMapPoint.h:110
void UpdateNormalAndDepth()
void ComputeDistinctiveDescriptors()
void SetWorldPos(const cv::Mat &Pos)
void log(const char *tag, const char *format,...)
logs a formatted string platform independently
Definition: Utils.cpp:1103
Initializer * initializer
Definition: WAISlamTools.h:19
std::vector< cv::Point2f > prevMatched
Definition: WAISlamTools.h:21
std::vector< cv::Point3f > iniPoint3D
Definition: WAISlamTools.h:22

◆ mapping()

void WAISlamTools::mapping ( WAIMap map,
LocalMap &  localMap,
LocalMapping *  localMapper,
WAIFrame frame,
int  inliers,
const unsigned long  lastRelocFrameId,
unsigned long &  lastKeyFrameFrameId 
)
static

Definition at line 507 of file WAISlamTools.cpp.

514 {
515  if (needNewKeyFrame(map, localMap, localMapper, frame, inliers, lastRelocFrameId, lastKeyFrameFrameId))
516  {
517  createNewKeyFrame(localMapper, localMap, map, frame, lastKeyFrameFrameId);
518  }
519 }
static bool needNewKeyFrame(WAIMap *globalMap, LocalMap &localMap, LocalMapping *localMapper, WAIFrame &frame, int nInliners, const unsigned long lastRelocFrameId, const unsigned long lastKeyFrameFrameId)
static void createNewKeyFrame(LocalMapping *localMapper, LocalMap &localMap, WAIMap *globalMap, WAIFrame &frame, unsigned long &lastKeyFrameFrameId)

◆ motionModel()

void WAISlamTools::motionModel ( WAIFrame frame,
WAIFrame lastFrame,
cv::Mat &  velocity,
cv::Mat &  pose 
)
static

Definition at line 535 of file WAISlamTools.cpp.

539 {
540  if (!lastFrame.mTcw.empty())
541  {
542  cv::Mat LastTwc = cv::Mat::eye(4, 4, CV_32F);
543  lastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0, 3).colRange(0, 3)); //mRwc
544 
545  //this is the translation of the frame w.r.t the world
546  const auto& cc = lastFrame.GetCameraCenter();
547 
548  cc.copyTo(LastTwc.rowRange(0, 3).col(3));
549 
550  velocity = frame.mTcw * LastTwc;
551  pose = frame.mTcw.clone();
552  }
553  else
554  {
555  velocity = cv::Mat();
556  }
557 }
cv::Mat mTcw
Definition: WAIFrame.h:155
cv::Mat GetCameraCenter()
Definition: WAIFrame.h:70
cv::Mat GetRotationInverse()
Definition: WAIFrame.h:76

◆ needNewKeyFrame()

bool WAISlamTools::needNewKeyFrame ( WAIMap globalMap,
LocalMap &  localMap,
LocalMapping *  localMapper,
WAIFrame frame,
int  nInliners,
const unsigned long  lastRelocFrameId,
const unsigned long  lastKeyFrameFrameId 
)
static

Definition at line 630 of file WAISlamTools.cpp.

637 {
638  if (localMapper->isPaused())
639  return false;
640 
641  const int nKFs = (int)map->KeyFramesInMap();
642 
643  // Do not insert keyframes if not enough frames have passed from last relocalisation
644  // this condition is only valid after MAX_FRAMES
645  if (frame.mnId < lastRelocFrameId + MAX_FRAMES && nKFs > MAX_FRAMES)
646  {
647  return false;
648  }
649 
650  // Tracked MapPoints in the reference keyframe
651  int nMinObs = 3;
652  if (nKFs <= 2)
653  nMinObs = 2;
654  int nRefMatches = localMap.refKF->TrackedMapPoints(nMinObs);
655 
656  // Thresholds
657  float thRefRatio = 0.9f;
658  if (nKFs < 2)
659  thRefRatio = 0.4f;
660 
661  // Condition 1a: More than "MaxFrames" have passed from last keyframe insertion
662  const bool c1a = frame.mnId >= lastKeyFrameFrameId + MAX_FRAMES;
663  // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
664  const bool c1b = (frame.mnId >= lastKeyFrameFrameId + MIN_FRAMES) && localMapper->AcceptKeyFrames();
665  // Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
666  const bool c2 = ((nInliers < nRefMatches * thRefRatio) && nInliers > 15);
667 
668  return ((c1a || c1b) && c2);
669 }
#define MIN_FRAMES
Definition: WAISlamTools.cpp:5
#define MAX_FRAMES
Definition: WAISlamTools.cpp:6

◆ oldInitialize()

bool WAISlamTools::oldInitialize ( WAIFrame frame,
WAIInitializerData iniData,
WAIMap map,
LocalMap &  localMap,
LocalMapping *  localMapper,
LoopClosing *  loopCloser,
WAIOrbVocabulary voc,
int  mapPointsNeeded = 100 
)
static

Definition at line 269 of file WAISlamTools.cpp.

277 {
278  int matchesNeeded = mapPointsNeeded;
279 
280  // Get Map Mutex -> Map cannot be changed
281  std::unique_lock<std::mutex> lock(map->mMutexMapUpdate, std::defer_lock);
282  lock.lock();
283 
284  if (!iniData.initializer)
285  {
286  // Set Reference Frame
287  if (frame.mvKeys.size() > matchesNeeded)
288  {
289  iniData.initialFrame = WAIFrame(frame);
290  iniData.prevMatched.resize(frame.mvKeysUn.size());
291  //ghm1: we store the undistorted keypoints of the initial frame in an extra vector
292  //todo: why not using mInitialFrame.mvKeysUn????
293  for (size_t i = 0; i < frame.mvKeysUn.size(); i++)
294  iniData.prevMatched[i] = frame.mvKeysUn[i].pt;
295 
296  iniData.initializer = new ORB_SLAM2::Initializer(frame, 1.0, 200);
297  //ghm1: clear mvIniMatches. it contains the index of the matched keypoint in the current frame
298  fill(iniData.iniMatches.begin(), iniData.iniMatches.end(), -1);
299  }
300  return false;
301  }
302  // Try to initialize
303  if ((int)frame.mvKeys.size() <= matchesNeeded)
304  {
305  delete iniData.initializer;
306  iniData.initializer = static_cast<Initializer*>(NULL);
307  fill(iniData.iniMatches.begin(), iniData.iniMatches.end(), -1);
308  return false;
309  }
310 
311  // Find correspondences
312  ORBmatcher matcher(0.9f, true);
313  int nmatches = matcher.SearchForInitialization(iniData.initialFrame, frame, iniData.prevMatched, iniData.iniMatches, 100);
314 
315  // Check if there are enough correspondences
316  if (nmatches < matchesNeeded)
317  {
318  delete iniData.initializer;
319  iniData.initializer = static_cast<Initializer*>(NULL);
320  return false;
321  }
322 
323  cv::Mat Rcw; // Current Camera Rotation
324  cv::Mat tcw; // Current Camera Translation
325  vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
326 
327  if (iniData.initializer->Initialize(frame, iniData.iniMatches, Rcw, tcw, iniData.iniPoint3D, vbTriangulated))
328  {
329  for (size_t i = 0, iend = iniData.iniMatches.size(); i < iend; i++)
330  {
331  if (iniData.iniMatches[i] >= 0 && !vbTriangulated[i])
332  {
333  iniData.iniMatches[i] = -1;
334  nmatches--;
335  }
336  }
337  }
338  else
339  {
340  return false;
341  }
342 
343  // Set Frame Poses
344  iniData.initialFrame.SetPose(cv::Mat::eye(4, 4, CV_32F));
345  cv::Mat Tcw = cv::Mat::eye(4, 4, CV_32F);
346  Rcw.copyTo(Tcw.rowRange(0, 3).colRange(0, 3));
347  tcw.copyTo(Tcw.rowRange(0, 3).col(3));
348  frame.SetPose(Tcw);
349 
351 
352  // Create KeyFrames
353  WAIKeyFrame* pKFini = new WAIKeyFrame(iniData.initialFrame);
354  WAIKeyFrame* pKFcur = new WAIKeyFrame(frame);
355 
356  pKFini->ComputeBoW(voc);
357  pKFcur->ComputeBoW(voc);
358 
359  // Insert KFs in the map
360  map->AddKeyFrame(pKFini);
361  map->AddKeyFrame(pKFcur);
362 
363  // Create MapPoints and asscoiate to keyframes
364  for (size_t i = 0; i < iniData.iniMatches.size(); i++)
365  {
366  if (iniData.iniMatches[i] < 0)
367  continue;
368 
369  //Create MapPoint.
370  cv::Mat worldPos(iniData.iniPoint3D[i]);
371 
372  WAIMapPoint* pMP = new WAIMapPoint(worldPos, pKFcur);
373 
374  pKFini->AddMapPoint(pMP, i);
375  pKFcur->AddMapPoint(pMP, iniData.iniMatches[i]);
376 
377  pMP->AddObservation(pKFini, i);
378  pMP->AddObservation(pKFcur, iniData.iniMatches[i]);
379 
381  pMP->UpdateNormalAndDepth();
382 
383  //Fill Current Frame structure
384  frame.mvpMapPoints[iniData.iniMatches[i]] = pMP;
385 
386  //Add to Map
387  map->AddMapPoint(pMP);
388  }
389 
390  // Update Connections
391  pKFini->FindAndUpdateConnections();
392  pKFcur->FindAndUpdateConnections();
393 
394  //cout << "New Map created with " << _map->MapPointsInMap() << " points" << endl;
395 
396  // Bundle Adjustment
397  Optimizer::GlobalBundleAdjustemnt(map, 20);
398 
399  // Set median depth to 1
400  float medianDepth = pKFini->ComputeSceneMedianDepth(2);
401  float invMedianDepth = 1.0f / medianDepth;
402 
403  if (medianDepth < 0 || pKFcur->TrackedMapPoints(1) < mapPointsNeeded)
404  {
405  Utils::log("WAI", "Wrong initialization, reseting...");
406  localMapper->RequestReset();
407  loopCloser->reset();
408 
409  map->clear();
410  localMap.keyFrames.clear();
411  localMap.mapPoints.clear();
412  localMap.refKF = nullptr;
413 
415  WAIFrame::nNextId = 0;
418  return false;
419  }
420 
421  localMap.keyFrames.push_back(pKFcur);
422  localMap.keyFrames.push_back(pKFini);
423 
424  // Scale initial baseline
425  cv::Mat Tc2w = pKFcur->GetPose();
426  Tc2w.col(3).rowRange(0, 3) = Tc2w.col(3).rowRange(0, 3) * invMedianDepth;
427  pKFcur->SetPose(Tc2w);
428 
429  // Scale points
430  vector<WAIMapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches();
431  for (size_t iMP = 0; iMP < vpAllMapPoints.size(); iMP++)
432  {
433  if (vpAllMapPoints[iMP])
434  {
435  WAIMapPoint* pMP = vpAllMapPoints[iMP];
436  pMP->SetWorldPos(pMP->GetWorldPos() * invMedianDepth);
437  }
438  }
439 
440  localMapper->InsertKeyFrame(pKFini);
441  localMapper->InsertKeyFrame(pKFcur);
442 
443  frame.SetPose(pKFcur->GetPose());
444  localMap.refKF = pKFcur;
445  localMap.mapPoints = map->GetAllMapPoints();
446 
447  frame.mpReferenceKF = pKFcur;
448 
449  map->SetReferenceMapPoints(localMap.mapPoints);
450  map->mvpKeyFrameOrigins.push_back(pKFini);
451  return true;
452 }
vector< WAIKeyFrame * > mvpKeyFrameOrigins
Definition: WAIMap.h:88
void clear()
Definition: WAIMap.cpp:158
void AddKeyFrame(WAIKeyFrame *pKF)
Definition: WAIMap.cpp:49
std::mutex mMutexMapUpdate
Definition: WAIMap.h:90
void SetReferenceMapPoints(const std::vector< WAIMapPoint * > &vpMPs)
Definition: WAIMap.cpp:86
void AddMapPoint(WAIMapPoint *pMP)
Definition: WAIMap.cpp:58

◆ relocalization()

bool WAISlamTools::relocalization ( WAIFrame currentFrame,
WAIMap waiMap,
LocalMap &  localMap,
float  minCommonWordFactor,
int &  inliers,
bool  minAccScoreFilter = false 
)
static

Definition at line 713 of file WAISlamTools.cpp.

719 {
720  AVERAGE_TIMING_START("relocalization");
721  // Compute Bag of Words Vector
722  currentFrame.ComputeBoW();
723  // Relocalization is performed when tracking is lost
724  // Track Lost: Query WAIKeyFrame Database for keyframe candidates for relocalisation
725  vector<WAIKeyFrame*> vpCandidateKFs;
726  vpCandidateKFs = waiMap->GetKeyFrameDB()->DetectRelocalizationCandidates(&currentFrame, minCommonWordFactor, minAccScoreFilter); //put boolean to argument
727  //vpCandidateKFs = waiMap->GetAllKeyFrames();
728  if (vpCandidateKFs.empty())
729  {
730  AVERAGE_TIMING_STOP("relocalization");
731  return false;
732  }
733 
734  //vector<WAIKeyFrame*> vpCandidateKFs = mpKeyFrameDatabase->keyFrames();
735  const int nKFs = (int)vpCandidateKFs.size();
736 
737  //DUtils::Random::SeedRandOnce(42);
738  //for(int i= 0; i < 10; ++ i)
739  // std::cout << "rand: " << DUtils::Random::RandomInt(0, 10) << std::endl;
740 
741  // We perform first an ORB matching with each candidate
742  // If enough matches are found we setup a PnP solver
743  // Best match < 0.75 * second best match (default is 0.6)
744  ORBmatcher matcher(0.75, true);
745 
746  vector<PnPsolver*> vpPnPsolvers;
747  vpPnPsolvers.resize(nKFs);
748 
749  vector<vector<WAIMapPoint*>> vvpMapPointMatches;
750  vvpMapPointMatches.resize(nKFs);
751 
752  std::vector<bool> outliers;
753 
754  vector<bool> vbDiscarded;
755  vbDiscarded.resize(nKFs);
756 
757  int nCandidates = 0;
758 
759  for (int i = 0; i < nKFs; i++)
760  {
761  WAIKeyFrame* pKF = vpCandidateKFs[i];
762  if (pKF->isBad())
763  vbDiscarded[i] = true;
764  else
765  {
766  int nmatches = matcher.SearchByBoW(pKF, currentFrame, vvpMapPointMatches[i]);
767  if (nmatches < 15)
768  {
769  vbDiscarded[i] = true;
770  continue;
771  }
772  else
773  {
774  PnPsolver* pSolver = new PnPsolver(currentFrame, vvpMapPointMatches[i]);
775  pSolver->SetRansacParameters(0.99, 10, 300, 4, 0.5f, 5.991f);
776  vpPnPsolvers[i] = pSolver;
777  nCandidates++;
778  }
779  }
780  }
781 
782  // Alternatively perform some iterations of P4P RANSAC
783  // Until we found a camera pose supported by enough inliers
784  bool bMatch = false;
785  ORBmatcher matcher2(0.9f, true);
786 
787  while (nCandidates > 0 && !bMatch)
788  {
789  for (int i = 0; i < nKFs; i++)
790  {
791  if (vbDiscarded[i])
792  continue;
793 
794  // Perform 5 Ransac Iterations
795  vector<bool> vbInliers;
796  int nInliers;
797  bool bNoMore;
798 
799  PnPsolver* pSolver = vpPnPsolvers[i];
800  cv::Mat Tcw = pSolver->iterate(5, bNoMore, vbInliers, nInliers);
801 
802  // If Ransac reachs max. iterations discard keyframe
803  if (bNoMore)
804  {
805  vbDiscarded[i] = true;
806  nCandidates--;
807  }
808 
809  // If a Camera Pose is computed, optimize
810  if (!Tcw.empty())
811  {
812  Tcw.copyTo(currentFrame.mTcw);
813 
814  set<WAIMapPoint*> sFound;
815 
816  const int np = (int)vbInliers.size();
817 
818  for (int j = 0; j < np; j++)
819  {
820  if (vbInliers[j])
821  {
822  currentFrame.mvpMapPoints[j] = vvpMapPointMatches[i][j];
823  sFound.insert(vvpMapPointMatches[i][j]);
824  }
825  else
826  currentFrame.mvpMapPoints[j] = NULL;
827  }
828 
829  int nGood = Optimizer::PoseOptimization(&currentFrame, outliers);
830 
831  if (nGood < 10)
832  {
833  //std::cout << "Number of nGood:" << nGood << std::endl;
834  continue;
835  }
836 
837  // If few inliers, search by projection in a coarse window and optimize again:
838  //ghm1: mappoints seen in the keyframe which was found as candidate via BoW-search are projected into
839  //the current frame using the position that was calculated using the matches from BoW matcher
840  if (nGood < 50)
841  {
842  int nadditional = matcher2.SearchByProjection(currentFrame, vpCandidateKFs[i], sFound, 10, 100);
843 
844  if (nadditional + nGood >= 50)
845  {
846  nGood = Optimizer::PoseOptimization(&currentFrame, outliers);
847 
848  // If many inliers but still not enough, search by projection again in a narrower window
849  // the camera has been already optimized with many points
850  if (nGood > 30 && nGood < 50)
851  {
852  sFound.clear();
853  for (int ip = 0; ip < currentFrame.N; ip++)
854  if (currentFrame.mvpMapPoints[ip] && !outliers[ip])
855  sFound.insert(currentFrame.mvpMapPoints[ip]);
856  nadditional = matcher2.SearchByProjection(currentFrame, vpCandidateKFs[i], sFound, 3, 64);
857 
858  // Final optimization
859  if (nGood + nadditional >= 50)
860  {
861  nGood = Optimizer::PoseOptimization(&currentFrame);
862  }
863  }
864  }
865  }
866 
867  // If the pose is supported by enough inliers stop ransacs and continue
868  if (nGood >= 50)
869  {
870  bMatch = trackLocalMap(localMap, currentFrame, (int)currentFrame.mnId, inliers);
871  //std::cout << "Number of nGood:" << nGood << std::endl;
872  break;
873  }
874  }
875  }
876  }
877 
878  AVERAGE_TIMING_STOP("relocalization");
879  return bMatch;
880 }
#define AVERAGE_TIMING_STOP(name)
Definition: AverageTiming.h:97
#define AVERAGE_TIMING_START(name)
Definition: AverageTiming.h:96
void ComputeBoW()
Definition: WAIFrame.cpp:317
std::vector< WAIKeyFrame * > DetectRelocalizationCandidates(WAIFrame *F, float minCommonWordFactor, bool applyMinAccScoreFilter=false)
WAIKeyFrameDB * GetKeyFrameDB()
Definition: WAIMap.h:78
static bool trackLocalMap(LocalMap &localMap, WAIFrame &frame, int lastRelocFrameId, int &inliers)

◆ relocalizationGPS()

bool WAISlamTools::relocalizationGPS ( WAIFrame currentFrame,
WAIMap waiMap,
LocalMap &  localMap,
cv::Mat  locENU,
cv::Mat  dirENU,
float  minCommonWordFactor,
int &  inliers,
bool  minAccScoreFilter 
)
static

Definition at line 882 of file WAISlamTools.cpp.

890 {
891  AVERAGE_TIMING_START("relocalization");
892  currentFrame.ComputeBoW();
893  // Relocalization is performed when tracking is lost
894  vector<WAIKeyFrame*> vpCandidateKFs;
895  vector<WAIKeyFrame*> allKf = waiMap->GetAllKeyFrames();
896 
897  float minDist;
898  for (WAIKeyFrame* kf : allKf)
899  {
900  cv::Mat tcw = kf->GetPose();
901  cv::Mat_<double> front(3, 1);
902 
903  front(0, 0) = 0;
904  front(1, 0) = 0;
905  front(2, 0) = -1.0;
906 
907  front = tcw * front;
908  if (dirENU.dot(front) < 0)
909  continue;
910 
911  cv::Mat pose = tcw.col(3).rowRange(0, 2);
912 
913  float dist = (float)cv::norm(locENU, pose);
914  if (dist < minDist)
915  minDist = dist;
916 
917  if (dist < 3)
918  vpCandidateKFs.push_back(kf);
919  }
920 
921  if (vpCandidateKFs.empty())
922  {
923  AVERAGE_TIMING_STOP("relocalization");
924  return false;
925  }
926 
927  const int nKFs = (int)vpCandidateKFs.size();
928 
929  // We perform first an ORB matching with each candidate
930  // If enough matches are found we setup a PnP solver
931  // Best match < 0.75 * second best match (default is 0.6)
932  ORBmatcher matcher(0.75, true);
933 
934  vector<PnPsolver*> vpPnPsolvers;
935  vpPnPsolvers.resize(nKFs);
936 
937  vector<vector<WAIMapPoint*>> vvpMapPointMatches;
938  vvpMapPointMatches.resize(nKFs);
939 
940  std::vector<bool> outliers;
941 
942  vector<bool> vbDiscarded;
943  vbDiscarded.resize(nKFs);
944 
945  int nCandidates = 0;
946 
947  for (int i = 0; i < nKFs; i++)
948  {
949  WAIKeyFrame* pKF = vpCandidateKFs[i];
950  if (pKF->isBad())
951  vbDiscarded[i] = true;
952  else
953  {
954  int nmatches = matcher.SearchByBoW(pKF, currentFrame, vvpMapPointMatches[i]);
955  if (nmatches < 15)
956  {
957  vbDiscarded[i] = true;
958  continue;
959  }
960  else
961  {
962  PnPsolver* pSolver = new PnPsolver(currentFrame, vvpMapPointMatches[i]);
963  pSolver->SetRansacParameters(0.99, 10, 300, 4, 0.5f, 5.991f);
964  vpPnPsolvers[i] = pSolver;
965  nCandidates++;
966  }
967  }
968  }
969 
970  // Alternatively perform some iterations of P4P RANSAC
971  // Until we found a camera pose supported by enough inliers
972  bool bMatch = false;
973  ORBmatcher matcher2(0.9f, true);
974 
975  while (nCandidates > 0 && !bMatch)
976  {
977  for (int i = 0; i < nKFs; i++)
978  {
979  if (vbDiscarded[i])
980  continue;
981 
982  // Perform 5 Ransac Iterations
983  vector<bool> vbInliers;
984  int nInliers;
985  bool bNoMore;
986 
987  PnPsolver* pSolver = vpPnPsolvers[i];
988  cv::Mat Tcw = pSolver->iterate(5, bNoMore, vbInliers, nInliers);
989 
990  // If Ransac reachs max. iterations discard keyframe
991  if (bNoMore)
992  {
993  vbDiscarded[i] = true;
994  nCandidates--;
995  }
996 
997  // If a Camera Pose is computed, optimize
998  if (!Tcw.empty())
999  {
1000  Tcw.copyTo(currentFrame.mTcw);
1001 
1002  set<WAIMapPoint*> sFound;
1003 
1004  const int np = (int)vbInliers.size();
1005 
1006  for (int j = 0; j < np; j++)
1007  {
1008  if (vbInliers[j])
1009  {
1010  currentFrame.mvpMapPoints[j] = vvpMapPointMatches[i][j];
1011  sFound.insert(vvpMapPointMatches[i][j]);
1012  }
1013  else
1014  currentFrame.mvpMapPoints[j] = NULL;
1015  }
1016 
1017  int nGood = Optimizer::PoseOptimization(&currentFrame, outliers);
1018 
1019  if (nGood < 10)
1020  {
1021  //std::cout << "Number of nGood:" << nGood << std::endl;
1022  continue;
1023  }
1024 
1025  // If few inliers, search by projection in a coarse window and optimize again:
1026  //ghm1: mappoints seen in the keyframe which was found as candidate via BoW-search are projected into
1027  //the current frame using the position that was calculated using the matches from BoW matcher
1028  if (nGood < 50)
1029  {
1030  int nadditional = matcher2.SearchByProjection(currentFrame, vpCandidateKFs[i], sFound, 10, 100);
1031 
1032  if (nadditional + nGood >= 50)
1033  {
1034  nGood = Optimizer::PoseOptimization(&currentFrame, outliers);
1035 
1036  // If many inliers but still not enough, search by projection again in a narrower window
1037  // the camera has been already optimized with many points
1038  if (nGood > 30 && nGood < 50)
1039  {
1040  sFound.clear();
1041  for (int ip = 0; ip < currentFrame.N; ip++)
1042  if (currentFrame.mvpMapPoints[ip] && !outliers[ip])
1043  sFound.insert(currentFrame.mvpMapPoints[ip]);
1044  nadditional = matcher2.SearchByProjection(currentFrame, vpCandidateKFs[i], sFound, 3, 64);
1045 
1046  // Final optimization
1047  if (nGood + nadditional >= 50)
1048  {
1049  nGood = Optimizer::PoseOptimization(&currentFrame);
1050  }
1051  }
1052  }
1053  }
1054 
1055  // If the pose is supported by enough inliers stop ransacs and continue
1056  if (nGood >= 50)
1057  {
1058  bMatch = trackLocalMap(localMap, currentFrame, (int)currentFrame.mnId, inliers);
1059  //std::cout << "Number of nGood:" << nGood << std::endl;
1060  break;
1061  }
1062  }
1063  }
1064  }
1065 
1066  AVERAGE_TIMING_STOP("relocalization");
1067  return bMatch;
1068 }

◆ strictMapping()

void WAISlamTools::strictMapping ( WAIMap map,
LocalMap &  localMap,
LocalMapping *  localMapper,
WAIFrame frame,
int  inliers,
const unsigned long  lastRelocFrameId,
unsigned long &  lastKeyFrameFrameId 
)
static

Definition at line 521 of file WAISlamTools.cpp.

528 {
529  if (strictNeedNewKeyFrame(map, localMap, localMapper, frame, inliers, lastRelocFrameId, lastKeyFrameFrameId))
530  {
531  createNewKeyFrame(localMapper, localMap, map, frame, lastKeyFrameFrameId);
532  }
533 }
static bool strictNeedNewKeyFrame(WAIMap *map, LocalMap &localMap, LocalMapping *localMapper, WAIFrame &frame, int nInliers, const unsigned long lastRelocFrameId, const unsigned long lastKeyFrameFrameId)

◆ strictNeedNewKeyFrame()

bool WAISlamTools::strictNeedNewKeyFrame ( WAIMap map,
LocalMap &  localMap,
LocalMapping *  localMapper,
WAIFrame frame,
int  nInliers,
const unsigned long  lastRelocFrameId,
const unsigned long  lastKeyFrameFrameId 
)
static

Definition at line 671 of file WAISlamTools.cpp.

678 {
679  if (localMapper->isPaused() || !localMapper->AcceptKeyFrames())
680  return false;
681 
682  const int nKFs = (int)map->KeyFramesInMap();
683 
684  // Do not insert keyframes if not enough frames have passed from last relocalisation
685  // this condition is only valid after MAX_FRAMES
686  if (frame.mnId < lastRelocFrameId + MAX_FRAMES && nKFs > MAX_FRAMES)
687  {
688  return false;
689  }
690 
691  // Tracked MapPoints in the reference keyframe
692  int nMinObs = 3;
693  if (nKFs <= 2)
694  nMinObs = 2;
695  int nRefMatches = localMap.refKF->TrackedMapPoints(nMinObs);
696 
697  unsigned int m, n, outliers;
698  bool c2 = nInliers > 45; //3000 features is 3x default => 45 is 3x 15
699  bool c3 = false;
700  // Count # of matched mappoint and also number of outliers from loaded map
701  countReprojectionOutliers(frame, m, n, outliers);
702  unsigned int nLoadedCorrect = n - outliers;
703  if (nLoadedCorrect > 0.25 * m)
704  {
705  if ((float)(nLoadedCorrect) / (float)n > 0.5f)
706  {
707  c3 = true;
708  }
709  }
710  return c2 && c3;
711 }
long unsigned int KeyFramesInMap()
Definition: WAIMap.cpp:116
static void countReprojectionOutliers(WAIFrame &frame, unsigned int &m, unsigned int &n, unsigned int &outliers)

◆ strictTracking()

bool WAISlamTools::strictTracking ( WAIMap map,
LocalMap &  localMap,
WAIFrame frame,
WAIFrame lastFrame,
int  lastRelocFrameId,
int &  inliers 
)
static

Definition at line 477 of file WAISlamTools.cpp.

483 {
484  inliers = 0;
485  if (!strictTrackReferenceKeyFrame(localMap, lastFrame, frame))
486  return false;
487 
488  return trackLocalMap(localMap, frame, lastRelocFrameId, inliers);
489 }
static bool strictTrackReferenceKeyFrame(LocalMap &map, WAIFrame &lastFrame, WAIFrame &frame)

◆ strictTrackReferenceKeyFrame()

bool WAISlamTools::strictTrackReferenceKeyFrame ( LocalMap &  map,
WAIFrame lastFrame,
WAIFrame frame 
)
static

Definition at line 1114 of file WAISlamTools.cpp.

1117 {
1118  //This routine is called if current tracking state is OK but we have NO valid motion model
1119  //1. Berechnung des BoW-Vectors für den current frame
1120  //2. using BoW we search mappoint matches (from reference keyframe) with orb in current frame (ORB that belong to the same vocabulary node (at a certain level))
1121  //3. if there are less than 15 matches return.
1122  //4. we use the pose found for the last frame as initial pose for the current frame
1123  //5. This pose is optimized using the matches to map points found by BoW search with reference frame
1124  //6. Matches classified as outliers by the optimization routine are updated in the mvpMapPoints vector in the current frame and the valid matches are counted
1125  //7. If there are more than 10 valid matches the reference frame tracking was successful.
1126 
1127  AVERAGE_TIMING_START("TrackReferenceKeyFrame");
1128 
1129  // Compute Bag of Words vector
1130  frame.ComputeBoW();
1131 
1132  // We perform first an ORB matching with the reference keyframe
1133  // If enough matches are found we setup a PnP solver
1134  ORBmatcher matcher(0.7f, true);
1135  vector<WAIMapPoint*> vpMapPointMatches;
1136 
1137  int nmatches = matcher.SearchByBoW(map.refKF, frame, vpMapPointMatches);
1138 
1139  if (nmatches < 15)
1140  {
1141  AVERAGE_TIMING_STOP("TrackReferenceKeyFrame");
1142  return false;
1143  }
1144 
1145  frame.mvpMapPoints = vpMapPointMatches;
1146  frame.SetPose(lastFrame.mTcw);
1147 
1148  nmatches = Optimizer::PoseOptimization(&frame);
1149 
1150  // Discard outliers
1151  // Now in the PoseOptimization itself the mappoint
1152  // in the frame is set to null already if the point is an outlier
1153 
1154  AVERAGE_TIMING_STOP("TrackReferenceKeyFrame");
1155  return nmatches >= 20;
1156 }

◆ tracking()

bool WAISlamTools::tracking ( WAIMap map,
LocalMap &  localMap,
WAIFrame frame,
WAIFrame lastFrame,
int  lastRelocFrameId,
cv::Mat &  velocity,
int &  inliers 
)
static

Definition at line 454 of file WAISlamTools.cpp.

461 {
462  //std::unique_lock<std::mutex> lock(map->mMutexMapUpdate, std::defer_lock);
463  //lock.lock();
464  inliers = 0;
465 
466  if (!trackWithMotionModel(velocity, lastFrame, frame))
467  {
468  if (!trackReferenceKeyFrame(localMap, lastFrame, frame))
469  {
470  return false;
471  }
472  }
473 
474  return trackLocalMap(localMap, frame, lastRelocFrameId, inliers);
475 }
static bool trackReferenceKeyFrame(LocalMap &map, WAIFrame &lastFrame, WAIFrame &frame)
static bool trackWithMotionModel(cv::Mat velocity, WAIFrame &previousFrame, WAIFrame &frame)

◆ trackLocalMap()

bool WAISlamTools::trackLocalMap ( LocalMap &  localMap,
WAIFrame frame,
int  lastRelocFrameId,
int &  inliers 
)
static

Definition at line 491 of file WAISlamTools.cpp.

495 {
496  updateLocalMap(frame, localMap);
497  if (!localMap.keyFrames.empty())
498  {
499  frame.mpReferenceKF = localMap.refKF;
500  inliers = trackLocalMapPoints(localMap, lastRelocFrameId, frame);
501  if (inliers > 50)
502  return true;
503  }
504  return false;
505 }
static void updateLocalMap(WAIFrame &frame, LocalMap &localMap)
static int trackLocalMapPoints(LocalMap &localMap, int lastRelocFrameId, WAIFrame &frame)

◆ trackLocalMapPoints()

int WAISlamTools::trackLocalMapPoints ( LocalMap &  localMap,
int  lastRelocFrameId,
WAIFrame frame 
)
static

Definition at line 1158 of file WAISlamTools.cpp.

1161 {
1162  // Do not search map points already matched
1163  for (vector<WAIMapPoint*>::iterator vit = frame.mvpMapPoints.begin(), vend = frame.mvpMapPoints.end(); vit != vend; vit++)
1164  {
1165  WAIMapPoint* pMP = *vit;
1166  if (pMP)
1167  {
1168  if (pMP->isBad())
1169  {
1170  *vit = static_cast<WAIMapPoint*>(NULL);
1171  }
1172  else
1173  {
1174  pMP->IncreaseVisible();
1175  //These field is only used in this function
1176  pMP->mnLastFrameSeen = frame.mnId;
1177  }
1178  }
1179  }
1180 
1181  int nToMatch = 0;
1182 
1183  // Project localmap points in frame and check its visibility
1184  for (vector<WAIMapPoint*>::iterator vit = localMap.mapPoints.begin(), vend = localMap.mapPoints.end(); vit != vend; vit++)
1185  {
1186  WAIMapPoint* pMP = *vit;
1187  if (pMP->mnLastFrameSeen == frame.mnId)
1188  continue;
1189  if (pMP->isBad())
1190  continue;
1191 
1192  if (frame.isInFrustum(pMP, 0.5))
1193  {
1194  pMP->IncreaseVisible();
1195  nToMatch++;
1196  }
1197  }
1198 
1199  if (nToMatch > 0)
1200  {
1201  ORBmatcher matcher(0.8f);
1202  int th = 1;
1203  // If the camera has been relocalised recently, perform a coarser search
1204  if (frame.mnId - 1 == lastRelocFrameId)
1205  th = 5;
1206  matcher.SearchByProjection(frame, localMap.mapPoints, (float)th);
1207  }
1208 
1209  // Optimize Pose
1210  Optimizer::PoseOptimization(&frame);
1211  int matchesInliers = 0;
1212 
1213  // Update MapPoints Statistics
1214  for (int i = 0; i < frame.N; i++)
1215  {
1216  if (frame.mvpMapPoints[i])
1217  {
1218  frame.mvpMapPoints[i]->IncreaseFound();
1219  if (frame.mvpMapPoints[i]->Observations() > 0)
1220  {
1221  matchesInliers++;
1222  }
1223  }
1224  }
1225 
1226  return matchesInliers;
1227 }
bool isInFrustum(WAIMapPoint *pMP, float viewingCosLimit)
Definition: WAIFrame.cpp:192
long unsigned int mnLastFrameSeen
Definition: WAIMapPoint.h:124
void IncreaseVisible(int n=1)

◆ trackReferenceKeyFrame()

bool WAISlamTools::trackReferenceKeyFrame ( LocalMap &  map,
WAIFrame lastFrame,
WAIFrame frame 
)
static

Definition at line 1070 of file WAISlamTools.cpp.

1073 {
1074  //This routine is called if current tracking state is OK but we have NO valid motion model
1075  //1. Berechnung des BoW-Vectors für den current frame
1076  //2. using BoW we search mappoint matches (from reference keyframe) with orb in current frame (ORB that belong to the same vocabulary node (at a certain level))
1077  //3. if there are less than 15 matches return.
1078  //4. we use the pose found for the last frame as initial pose for the current frame
1079  //5. This pose is optimized using the matches to map points found by BoW search with reference frame
1080  //6. Matches classified as outliers by the optimization routine are updated in the mvpMapPoints vector in the current frame and the valid matches are counted
1081  //7. If there are more than 10 valid matches the reference frame tracking was successful.
1082 
1083  AVERAGE_TIMING_START("TrackReferenceKeyFrame");
1084 
1085  // Compute Bag of Words vector
1086  frame.ComputeBoW();
1087 
1088  // We perform first an ORB matching with the reference keyframe
1089  // If enough matches are found we setup a PnP solver
1090  ORBmatcher matcher(0.7f, true);
1091  vector<WAIMapPoint*> vpMapPointMatches;
1092 
1093  int nmatches = matcher.SearchByBoW(map.refKF, frame, vpMapPointMatches);
1094 
1095  if (nmatches < 15)
1096  {
1097  AVERAGE_TIMING_STOP("TrackReferenceKeyFrame");
1098  return false;
1099  }
1100 
1101  frame.mvpMapPoints = vpMapPointMatches;
1102  frame.SetPose(lastFrame.mTcw);
1103 
1104  nmatches = Optimizer::PoseOptimization(&frame);
1105 
1106  // Discard outliers
1107  // Now in the PoseOptimization itself the mappoint
1108  // in the frame is set to null already if the point is an outlier
1109 
1110  AVERAGE_TIMING_STOP("TrackReferenceKeyFrame");
1111  return nmatches >= 10;
1112 }

◆ trackWithMotionModel()

bool WAISlamTools::trackWithMotionModel ( cv::Mat  velocity,
WAIFrame previousFrame,
WAIFrame frame 
)
static

Definition at line 1354 of file WAISlamTools.cpp.

1357 {
1358  AVERAGE_TIMING_START("TrackWithMotionModel");
1359 
1360  if (velocity.empty() || frame.mnId > previousFrame.mnId + 1)
1361  return false;
1362 
1363  ORBmatcher matcher(0.9f, true);
1364 
1365  // Update last frame pose according to its reference keyframe
1366  // Create "visual odometry" points if in Localization Mode
1367  //WAIKeyFrame* pRef = previousFrame.mpReferenceKF;
1368  //previousFrame.SetPose(last.lastFrameRelativePose * pRef->GetPose());
1369 
1370  //this adds the motion difference between the last and the before-last frame to the pose of the last frame to estimate the position of the current frame
1371 
1372  frame.SetPose(velocity * previousFrame.mTcw);
1373 
1374  fill(frame.mvpMapPoints.begin(), frame.mvpMapPoints.end(), static_cast<WAIMapPoint*>(NULL));
1375 
1376  // Project points seen in previous frame
1377  int th = 15;
1378  int nmatches = matcher.SearchByProjection(frame, previousFrame, (float)th, true);
1379 
1380  // If few matches, uses a wider window search
1381  if (nmatches < 20)
1382  {
1383  fill(frame.mvpMapPoints.begin(), frame.mvpMapPoints.end(), static_cast<WAIMapPoint*>(NULL));
1384  nmatches = matcher.SearchByProjection(frame, previousFrame, (float)(2 * th), true);
1385  }
1386 
1387  if (nmatches < 20)
1388  {
1389  AVERAGE_TIMING_STOP("TrackWithMotionModel");
1390  return false;
1391  }
1392 
1393  // Optimize frame pose with all matches
1394  nmatches = Optimizer::PoseOptimization(&frame);
1395 
1396  AVERAGE_TIMING_STOP("TrackWithMotionModel");
1397 
1398  return nmatches >= 10;
1399 }

◆ updateLocalMap()

void WAISlamTools::updateLocalMap ( WAIFrame frame,
LocalMap &  localMap 
)
static

Definition at line 1229 of file WAISlamTools.cpp.

1231 {
1232  // Each map point vote for the keyframes in which it has been observed
1233  map<WAIKeyFrame*, int> keyframeCounter;
1234  for (int i = 0; i < frame.N; i++)
1235  {
1236  if (frame.mvpMapPoints[i])
1237  {
1238  WAIMapPoint* pMP = frame.mvpMapPoints[i];
1239  if (!pMP->isBad())
1240  {
1241  const map<WAIKeyFrame*, size_t> observations = pMP->GetObservations();
1242  for (map<WAIKeyFrame*, size_t>::const_iterator it = observations.begin(), itend = observations.end(); it != itend; it++)
1243  keyframeCounter[it->first]++;
1244  }
1245  else
1246  {
1247  frame.mvpMapPoints[i] = NULL;
1248  }
1249  }
1250  }
1251 
1252  if (keyframeCounter.empty())
1253  return;
1254 
1255  int max = 0;
1256  localMap.refKF = static_cast<WAIKeyFrame*>(NULL);
1257 
1258  localMap.keyFrames.clear();
1259  localMap.keyFrames.reserve(3 * keyframeCounter.size());
1260 
1261  // All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points
1262 
1263  for (auto it = keyframeCounter.begin(), itEnd = keyframeCounter.end(); it != itEnd; it++)
1264  {
1265  WAIKeyFrame* pKF = it->first;
1266 
1267  if (pKF->isBad())
1268  continue;
1269 
1270  if (it->second > max) //Should be computed with parent and child added to localocalMap
1271  {
1272  max = it->second;
1273  localMap.refKF = pKF;
1274  }
1275 
1276  localMap.keyFrames.push_back(it->first);
1277  pKF->mnMarker[TRACK_REF_FRAME] = frame.mnId;
1278  }
1279 
1280  // Include also some not-already-included keyframes that are neighbors to already-included keyframes
1281  for (int i = 0, iend = (int)localMap.keyFrames.size(); i < iend; ++i)
1282  {
1283  // Limit the number of keyframes
1284  if (localMap.keyFrames.size() > 80)
1285  break;
1286 
1287  WAIKeyFrame* pKF = localMap.keyFrames[i];
1288 
1289  const vector<WAIKeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
1290 
1291  for (vector<WAIKeyFrame*>::const_iterator itNeighKF = vNeighs.begin(), itEndNeighKF = vNeighs.end(); itNeighKF != itEndNeighKF; itNeighKF++)
1292  {
1293  WAIKeyFrame* pNeighKF = *itNeighKF;
1294  if (!pNeighKF->isBad())
1295  {
1296  if (pNeighKF->mnMarker[TRACK_REF_FRAME] != frame.mnId) //to ensure not already added at previous step
1297  {
1298  localMap.keyFrames.push_back(pNeighKF);
1299  pNeighKF->mnMarker[TRACK_REF_FRAME] = frame.mnId;
1300  break;
1301  }
1302  }
1303  }
1304 
1305  const set<WAIKeyFrame*> spChilds = pKF->GetChilds();
1306  for (set<WAIKeyFrame*>::const_iterator sit = spChilds.begin(), send = spChilds.end(); sit != send; sit++)
1307  {
1308  WAIKeyFrame* pChildKF = *sit;
1309  if (!pChildKF->isBad())
1310  {
1311  if (pChildKF->mnMarker[TRACK_REF_FRAME] != frame.mnId)
1312  {
1313  localMap.keyFrames.push_back(pChildKF);
1314  pChildKF->mnMarker[TRACK_REF_FRAME] = frame.mnId;
1315  break;
1316  }
1317  }
1318  }
1319 
1320  WAIKeyFrame* pParent = pKF->GetParent();
1321  if (pParent)
1322  {
1323  if (pParent->mnMarker[TRACK_REF_FRAME] != frame.mnId)
1324  {
1325  localMap.keyFrames.push_back(pParent);
1326  pParent->mnMarker[TRACK_REF_FRAME] = frame.mnId;
1327  break;
1328  }
1329  }
1330  }
1331 
1332  localMap.mapPoints.clear();
1333  for (int i = 0; i < localMap.keyFrames.size(); ++i)
1334  {
1335  WAIKeyFrame* pKF = localMap.keyFrames[i];
1336  const vector<WAIMapPoint*> vpMPs = pKF->GetMapPointMatches();
1337 
1338  for (vector<WAIMapPoint*>::const_iterator itMP = vpMPs.begin(), itEndMP = vpMPs.end(); itMP != itEndMP; itMP++)
1339  {
1340  WAIMapPoint* pMP = *itMP;
1341  if (!pMP)
1342  continue;
1343  if (pMP->mnMarker[TRACK_REF_FRAME] == frame.mnId)
1344  continue;
1345  if (!pMP->isBad())
1346  {
1347  localMap.mapPoints.push_back(pMP);
1348  pMP->mnMarker[TRACK_REF_FRAME] = (int)frame.mnId;
1349  }
1350  }
1351  }
1352 }
#define TRACK_REF_FRAME
Definition: WAIMapPoint.h:31
std::vector< WAIKeyFrame * > GetBestCovisibilityKeyFrames(const int &N)
WAIKeyFrame * GetParent()
long unsigned int mnMarker[7]
Definition: WAIKeyFrame.h:199
std::map< WAIKeyFrame *, size_t > GetObservations()
int mnMarker[7]
Definition: WAIMapPoint.h:128

Member Data Documentation

◆ _cameraExtrinsic

cv::Mat WAISlamTools::_cameraExtrinsic
protected

Definition at line 202 of file WAISlamTools.h.

◆ _cameraExtrinsicGuess

cv::Mat WAISlamTools::_cameraExtrinsicGuess
protected

Definition at line 204 of file WAISlamTools.h.

◆ _cameraIntrinsic

cv::Mat WAISlamTools::_cameraIntrinsic
protected

Definition at line 201 of file WAISlamTools.h.

◆ _distortion

cv::Mat WAISlamTools::_distortion
protected

Definition at line 200 of file WAISlamTools.h.

◆ _globalMap

std::unique_ptr<WAIMap> WAISlamTools::_globalMap
protected

Definition at line 208 of file WAISlamTools.h.

◆ _iniData

WAIInitializerData WAISlamTools::_iniData
protected

Definition at line 205 of file WAISlamTools.h.

◆ _initialized

bool WAISlamTools::_initialized = false
protected

Definition at line 212 of file WAISlamTools.h.

◆ _lastFrame

WAIFrame WAISlamTools::_lastFrame
protected

Definition at line 206 of file WAISlamTools.h.

◆ _localMap

LocalMap WAISlamTools::_localMap
protected

Definition at line 209 of file WAISlamTools.h.

◆ _localMapping

LocalMapping* WAISlamTools::_localMapping = nullptr
protected

Definition at line 214 of file WAISlamTools.h.

◆ _loopClosing

LoopClosing* WAISlamTools::_loopClosing = nullptr
protected

Definition at line 215 of file WAISlamTools.h.

◆ _loopClosingThread

std::thread* WAISlamTools::_loopClosingThread = nullptr
protected

Definition at line 218 of file WAISlamTools.h.

◆ _mappingThreads

std::vector<std::thread*> WAISlamTools::_mappingThreads
protected

Definition at line 217 of file WAISlamTools.h.

◆ _processNewKeyFrameThread

std::thread* WAISlamTools::_processNewKeyFrameThread = nullptr
protected

Definition at line 216 of file WAISlamTools.h.

◆ _velocity

cv::Mat WAISlamTools::_velocity
protected

Definition at line 211 of file WAISlamTools.h.

◆ _voc

WAIOrbVocabulary* WAISlamTools::_voc = nullptr
protected

Definition at line 210 of file WAISlamTools.h.


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