From 10069d961d21e5f74808e44fcb0cf099e4bed8c0 Mon Sep 17 00:00:00 2001 From: Carlos Campos Date: Thu, 23 Jul 2020 17:12:55 +0200 Subject: [PATCH] Remove unused code --- include/LucasKanadeTracker.h | 60 ------ include/Tracking.h | 1 - src/LoopClosing.cc | 6 +- src/LucasKanadeTracker.cc | 393 ----------------------------------- src/Tracking.cc | 2 +- 5 files changed, 4 insertions(+), 458 deletions(-) delete mode 100644 include/LucasKanadeTracker.h delete mode 100644 src/LucasKanadeTracker.cc diff --git a/include/LucasKanadeTracker.h b/include/LucasKanadeTracker.h deleted file mode 100644 index cdf732afe3..0000000000 --- a/include/LucasKanadeTracker.h +++ /dev/null @@ -1,60 +0,0 @@ -//***************************************************************** -// File: LucasKanadeTracker.h -// Author: Juan José Gómez Rodríguez (jjgomez96@hotmail.com) -// Date: 01/05/2019 -// Coms: Header file of the Lucas-Kanade optical flow algorithm -//***************************************************************** - -#ifndef KLT_H -#define KLT_H - -#include -#include "opencv2/opencv.hpp" - -class LucasKanadeTracker { -public: - /* - * Default constructor - */ - LucasKanadeTracker(); - - /* - * Constructor with parameters - */ - LucasKanadeTracker(const cv::Size _winSize, const int _maxLevel, const int _maxIters, - const float _epsilon, const float _minEigThreshold); - - /* - * - */ - void SetReferenceImage(cv::Mat &refIm, std::vector &refPts); - - int PRE_Track(cv::Mat &newIm, - std::vector &nextPts, std::vector &status, const bool bInitialFlow, - const bool bCheckLength); - - void DeleteRefPt(const int idx); - cv::Point2f GetPrevPoint(const int idx); - -private: - //------------------------------- - // KLT parameters - //------------------------------- - cv::Size winSize; //size of the integration window of the Lucas-Kanade algorithm - int maxLevel; //max level of the image pyramids - int maxIters; //max number of iterations of the optical flow algorithm - float epsilon; //minimum optical flow imposed displacement. If lower, we stop computing - float minEigThreshold; //min eigen threshold value for the Spatial Gradient matrix - - //------------------------------- - // Pre computed stuff - //------------------------------- - std::vector prevPts; //Original coordinates of the points to track - std::vector refPyr; //Reference pyramid - std::vector> vMeanI; //Reference window mean intensities - std::vector> vMeanI2; //Reference window mean sqared intensities - std::vector> Iref; //Reference windows - std::vector> Idref; //Reference derivative windows -}; - -#endif //KLT_H diff --git a/include/Tracking.h b/include/Tracking.h index 2447c4f1f9..6ab09cfae7 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -37,7 +37,6 @@ #include "MapDrawer.h" #include "System.h" #include "ImuTypes.h" -#include "LucasKanadeTracker.h" #include "GeometricCamera.h" diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc index 4b9514c71d..f85b8e12c2 100644 --- a/src/LoopClosing.cc +++ b/src/LoopClosing.cc @@ -566,9 +566,9 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, { nBoWMatches = 20; nBoWInliers = 15; - nSim3Inliers = 30; - nProjMatches = 50; - nProjOptMatches = 70; + nSim3Inliers = 20; + nProjMatches = 35; + nProjOptMatches = 50; }*/ set spConnectedKeyFrames = mpCurrentKF->GetConnectedKeyFrames(); diff --git a/src/LucasKanadeTracker.cc b/src/LucasKanadeTracker.cc deleted file mode 100644 index 153cebb524..0000000000 --- a/src/LucasKanadeTracker.cc +++ /dev/null @@ -1,393 +0,0 @@ -//***************************************************************** -// File: LucasKanadeTracker.cpp -// Author: Juan José Gómez Rodríguez (jjgomez96@hotmail.com) -// Date: 01/05/2019 -// Coms: Implementation file of the Lucas-Kanade optical flow algorithm -//***************************************************************** - -#include "LucasKanadeTracker.h" - -#include - -using namespace std; -using namespace cv; - -#define CV_MAKETYPE(depth,cn) (CV_MAT_DEPTH(depth) + (((cn)-1) << CV_CN_SHIFT)) -#define CV_DESCALE(x,n) (((x) + (1 << ((n)-1))) >> (n)) - - -LucasKanadeTracker::LucasKanadeTracker() : winSize(Size(21, 21)), maxLevel(3), maxIters(30), epsilon(0.01), - minEigThreshold(1e-4) {} - -LucasKanadeTracker::LucasKanadeTracker(const cv::Size _winSize, const int _maxLevel, const int _maxIters, - const float _epsilon, const float _minEigThreshold) : - winSize(_winSize), maxLevel(_maxLevel), maxIters(_maxIters), epsilon(_epsilon), - minEigThreshold(_minEigThreshold) {} - -void LucasKanadeTracker::SetReferenceImage(Mat &refIm, vector &refPts) { - //Compute reference pyramid - cv::buildOpticalFlowPyramid(refIm, refPyr, winSize, maxLevel); - - //Store points - prevPts = refPts; - - vMeanI = vector>(maxLevel + 1, vector(refPts.size())); - vMeanI2 = vector>(maxLevel + 1, vector(refPts.size())); - Iref = vector>(maxLevel + 1, vector(refPts.size())); - Idref = vector>(maxLevel + 1, vector(refPts.size())); - - //Compute reference windows (intensity and derivtives) and means of the windows - Point2f halfWin((winSize.width - 1) * 0.5f, (winSize.height - 1) * 0.5f); - - for (int level = maxLevel; level >= 0; level--) { - //Get images form the pyramid - const Mat I = refPyr[level * 2]; - const Mat derivI = refPyr[level * 2 + 1]; - - //Steps for matrix indexing - int dstep = (int) (derivI.step / derivI.elemSize1()); - int stepI = (int) (I.step / I.elemSize1()); - - //Buffer for fast memory access - int cn = I.channels(), cn2 = cn * 2; //cn should be 1 therefor cn2 should be 2 - AutoBuffer _buf(winSize.area() * (cn + cn2)); - int derivDepth = DataType::depth; - - //Integration window buffers - Mat IWinBuf(winSize, CV_MAKETYPE(derivDepth, cn), (*_buf)); - Mat derivIWinBuf(winSize, CV_MAKETYPE(derivDepth, cn2), (*_buf) + winSize.area() * cn); - - for (int i = 0; i < prevPts.size(); i++) { - //Compute image coordinates in the reference image at the current level - Point2f point = prevPts[i] / (1 << level); - - Point2i ipoint; - point -= halfWin; - ipoint.x = cvFloor(point.x); - ipoint.y = cvFloor(point.y); - - if (ipoint.x < -winSize.width || ipoint.x >= derivI.cols || - ipoint.y < -winSize.height || ipoint.y >= derivI.rows) { - continue; - } - - //Compute weighs for sub pixel computation - float a = point.x - ipoint.x; - float b = point.y - ipoint.y; - const int W_BITS = 14, W_BITS1 = 14; - const float FLT_SCALE = 1.f / (1 << 20); - int iw00 = cvRound((1.f - a) * (1.f - b) * (1 << W_BITS)); - int iw01 = cvRound(a * (1.f - b) * (1 << W_BITS)); - int iw10 = cvRound((1.f - a) * b * (1 << W_BITS)); - int iw11 = (1 << W_BITS) - iw00 - iw01 - iw10; - - //Compute sumI, sumI2, meanI, meanI2, Iwin, IdWin - float meanI = 0.f, meanI2 = 0.f; - - int x, y; - for (y = 0; y < winSize.height; y++) { - //Get pointers to the images - const uchar *src = I.ptr() + (y + ipoint.y) * stepI + ipoint.x; - const short *dsrc = derivI.ptr() + (y + ipoint.y) * dstep + ipoint.x * 2; - - //Get pointers to the window buffers - short *Iptr = IWinBuf.ptr(y); - short *dIptr = derivIWinBuf.ptr(y); - - x = 0; - for (; x < winSize.width * cn; x++, dsrc += 2, dIptr += 2) { - //Get sub pixel values from images - int ival = CV_DESCALE(src[x] * iw00 + src[x + cn] * iw01 + - src[x + stepI] * iw10 + src[x + stepI + cn] * iw11, W_BITS1 - 5); - int ixval = CV_DESCALE(dsrc[0] * iw00 + dsrc[cn2] * iw01 + - dsrc[dstep] * iw10 + dsrc[dstep + cn2] * iw11, W_BITS1); - int iyval = CV_DESCALE(dsrc[1] * iw00 + dsrc[cn2 + 1] * iw01 + dsrc[dstep + 1] * iw10 + - dsrc[dstep + cn2 + 1] * iw11, W_BITS1); - - //Store values to the window buffers - Iptr[x] = (short) ival; - dIptr[0] = (short) ixval; - dIptr[1] = (short) iyval; - - //Compute accum values for later gain and bias computation - meanI += (float) ival; - meanI2 += (float) (ival * ival); - } - } - //Compute means for later gain and bias computation - vMeanI[level][i] = (meanI * FLT_SCALE) / winSize.area(); - vMeanI2[level][i] = (meanI2 * FLT_SCALE) / winSize.area(); - - Iref[level][i] = IWinBuf.clone(); - Idref[level][i] = derivIWinBuf.clone(); - } - } - -} - -int LucasKanadeTracker::PRE_Track(Mat &newIm, - std::vector &nextPts, vector &status, const bool bInitialFlow, - const bool bCheckLength) { - //Set status of all the points to true - status = vector(prevPts.size(), true); - - if (!bInitialFlow) - nextPts = vector(prevPts.size()); - - //Dimensions of half of the window - Point2f halfWin((winSize.width - 1) * 0.5f, (winSize.height - 1) * 0.5f); - - //Compute pyramid images - vector newPyr; - cv::buildOpticalFlowPyramid(newIm, newPyr, winSize, maxLevel); - - //Start Lucas-Kanade optical flow algorithm - //First iterate over pyramid levels - for (int level = maxLevel; level >= 0; level--) { - //Get images and gradients - const Mat I = refPyr[level * 2]; - const Mat J = newPyr[level * 2]; - const Mat derivI = refPyr[level * 2 + 1]; - const Mat derivJ = newPyr[level * 2 + 1]; - - //Buffer for fast memory access - int j, cn = I.channels(), cn2 = cn * 2; //cn should be 1 therefor cn2 should be 2 - AutoBuffer _buf(winSize.area() * (cn + cn2) * 2); - int derivDepth = DataType::depth; - - //Integration window buffers - Mat IWinBuf(winSize, CV_MAKETYPE(derivDepth, cn), (*_buf)); - Mat JWinBuf(winSize, CV_MAKETYPE(derivDepth, cn), (*_buf) + winSize.area()); - Mat derivIWinBuf(winSize, CV_MAKETYPE(derivDepth, cn2), (*_buf) + 2 * winSize.area()); - Mat derivJWinBuf(winSize, CV_MAKETYPE(derivDepth, cn2), (*_buf) + 4 * winSize.area()); - - //Steps for matrix indexing - int dstep = (int) (derivI.step / derivI.elemSize1()); - int stepJ = (int) (J.step / J.elemSize1()); - - //Track each point at the current pyramid level - for (int i = 0; i < prevPts.size(); i++) { - //Compute image coordinates in the reference image at the current level - Point2f prevPt = prevPts[i] * (float) (1. / (1 << level)); - //Compute image coordinates in the current frame at the current level - Point2f nextPt; - if (level == maxLevel) { - if (bInitialFlow) { - nextPt = nextPts[i] * (float) (1. / (1 << level)); - } else { - nextPt = prevPt; - } - } else { - nextPt = nextPts[i] * 2.f; - } - - nextPts[i] = nextPt; - - //Check that previous point and next point is inside of the - //image boundaries - Point2i iprevPt, inextPt; - prevPt -= halfWin; - iprevPt.x = cvFloor(prevPt.x); - iprevPt.y = cvFloor(prevPt.y); - - if (iprevPt.x < -winSize.width || iprevPt.x >= derivI.cols || - iprevPt.y < -winSize.height || iprevPt.y >= derivI.rows) { - if (level == 0) - status[i] = false; - - continue; - } - - //Compute weighs for sub pixel computation - const int W_BITS = 14, W_BITS1 = 14; - const float FLT_SCALE = 1.f / (1 << 20); - - //Compute sumI, sumI2, meanI, meanI2, Iwin, IdWin - float meanI = 0.f, meanI2 = 0.f; - - int x, y; - //Compute means for later gain and bias computation - meanI = vMeanI[level][i]; - meanI2 = vMeanI2[level][i]; - - IWinBuf = Iref[level][i].clone(); - derivIWinBuf = Idref[level][i].clone(); - - //Optical flow loop - Point2f prevDelta; - nextPt -= halfWin; - for (j = 0; j < maxIters; j++) { - //Compute weighs for sub pixel computation - inextPt.x = cvFloor(nextPt.x); - inextPt.y = cvFloor(nextPt.y); - - //Check that the point is inside the image - if (inextPt.x < -winSize.width || inextPt.x >= J.cols || - inextPt.y < -winSize.height || inextPt.y >= J.rows) { - if (level == 0) - status[i] = false; - break; - } - - float aJ = nextPt.x - inextPt.x; - float bJ = nextPt.y - inextPt.y; - int jw00 = cvRound((1.f - aJ) * (1.f - bJ) * (1 << W_BITS)); - int jw01 = cvRound(aJ * (1.f - bJ) * (1 << W_BITS)); - int jw10 = cvRound((1.f - aJ) * bJ * (1 << W_BITS)); - int jw11 = (1 << W_BITS) - jw00 - jw01 - jw10; - - //Compute alpha and beta for gain and bias - //Compute sumI, sumI2, meanI, meanI2, Iwin, IdWin - float meanJ = 0.f, meanJ2 = 0.f; - - for (y = 0; y < winSize.height; y++) { - //Get pointers to the images - const uchar *src = J.ptr() + (y + inextPt.y) * stepJ + inextPt.x * cn; - const short *dsrc = derivJ.ptr() + (y + inextPt.y) * dstep + inextPt.x * cn2; - - //Get pointers to the window buffers - short *Jptr = JWinBuf.ptr(y); - short *dJptr = derivJWinBuf.ptr(y); - - x = 0; - - for (; x < winSize.width * cn; x++, dsrc += 2, dJptr += 2) { - //Get sub pixel values from images - int jval = CV_DESCALE(src[x] * jw00 + src[x + cn] * jw01 + - src[x + stepJ] * jw10 + src[x + stepJ + cn] * jw11, W_BITS1 - 5); - int jxval = CV_DESCALE(dsrc[0] * jw00 + dsrc[cn2] * jw01 + - dsrc[dstep] * jw10 + dsrc[dstep + cn2] * jw11, W_BITS1); - int jyval = CV_DESCALE(dsrc[1] * jw00 + dsrc[cn2 + 1] * jw01 + dsrc[dstep + 1] * jw10 + - dsrc[dstep + cn2 + 1] * jw11, W_BITS1); - - //Store values to the window buffers - Jptr[x] = (short) jval; - dJptr[0] = (short) jxval; - dJptr[1] = (short) jyval; - - //Compute accum values for later gain and bias computation - meanJ += (float) jval; - meanJ2 += (float) (jval * jval); - } - } - - //Compute means for later gain and bias computation - meanJ = (meanJ * FLT_SCALE) / winSize.area(); - meanJ2 = (meanJ2 * FLT_SCALE) / winSize.area(); - - //Compute alpha and beta - float alpha = sqrt(meanI2 / meanJ2); - float beta = meanI - alpha * meanJ; - - //Compute image gradient insensitive to ilumination changes - float ib1 = 0, ib2 = 0; - float b1, b2; - float iA11 = 0, iA12 = 0, iA22 = 0; - float A11, A12, A22; - - for (y = 0; y < winSize.height; y++) { - //Get pointers to the buffers - const short *Iptr = IWinBuf.ptr(y); - const short *Jptr = JWinBuf.ptr(y); - const short *dIptr = derivIWinBuf.ptr(y); - const short *dJptr = derivJWinBuf.ptr(y); - - x = 0; - for (; x < winSize.width * cn; x++, dIptr += 2, dJptr += 2) { - int diff = Jptr[x] * alpha - Iptr[x] - beta; - float dx = (float) (dIptr[0] + dJptr[0] * alpha); - float dy = (float) (dIptr[1] + dJptr[1] * alpha); - - ib1 += (float) (diff * dx); - ib2 += (float) (diff * dy); - - iA11 += (float) (dx * dx); - iA22 += (float) (dy * dy); - iA12 += (float) (dx * dy); - } - } - b1 = ib1 * FLT_SCALE; - b2 = ib2 * FLT_SCALE; - - //Compute spatial gradient matrix - A11 = iA11 * FLT_SCALE; - A12 = iA12 * FLT_SCALE; - A22 = iA22 * FLT_SCALE; - - float D = A11 * A22 - A12 * A12; - float minEig = (A22 + A11 - std::sqrt((A11 - A22) * (A11 - A22) + - 4.f * A12 * A12)) / (2 * winSize.width * winSize.height); - - if (minEig < minEigThreshold || D < FLT_EPSILON) { - if (level == 0) - status[i] = false; - continue; - } - - D = 1.f / D; - - //Compute optical flow - Point2f delta((float) ((A12 * b2 - A22 * b1) * D), - (float) ((A12 * b1 - A11 * b2) * D)); - - nextPt += delta; - nextPts[i] = nextPt + halfWin; - - if (delta.ddot(delta) <= epsilon) - break; - - if (j > 0 && std::abs(delta.x + prevDelta.x) < 0.01 && - std::abs(delta.y + prevDelta.y) < 0.01) { - nextPts[i] -= delta * 0.5f; - break; - } - prevDelta = delta; - } - } - } - - //Track lenght check - if (bCheckLength) { - //Compute total displacement of aeach good tracked point - vector deltas, deltasShorted; - for (int i = 0; i < prevPts.size(); i++) { - if (!status[i]) { - deltas.push_back(0.f); - continue; - } - Point2f pDelta = nextPts[i] - prevPts[i]; - deltas.push_back(sqrt(pDelta.x * pDelta.x + pDelta.y * pDelta.y)); - deltasShorted.push_back(sqrt(pDelta.x * pDelta.x + pDelta.y * pDelta.y)); - } - sort(deltasShorted.begin(), deltasShorted.end()); - float median = deltasShorted[deltasShorted.size() / 2]; - - if(median < 20.f){ - return deltasShorted.size(); - } - - for (int i = deltas.size() - 1; i >= 0; i--) { - if (deltas[i] > median * 2) - status[i] = false; - } - return deltasShorted.size(); - } -} - -void LucasKanadeTracker::DeleteRefPt(const int idx) -{ - prevPts.erase(prevPts.begin()+idx); - for(int level = maxLevel; level >= 0; level--) - { - vMeanI[level].erase(vMeanI[level].begin()+idx); - vMeanI2[level].erase(vMeanI2[level].begin()+idx); - Iref[level].erase(Iref[level].begin()+idx); - Idref[level].erase(Idref[level].begin()+idx); - - } -} - -cv::Point2f LucasKanadeTracker::GetPrevPoint(const int idx) -{ - return prevPts[idx]; -} diff --git a/src/Tracking.cc b/src/Tracking.cc index 3c91b6137d..d6f82fca1a 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -49,7 +49,7 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, mState(NO_IMAGES_YET), mSensor(sensor), mTrackedFr(0), mbStep(false), mbOnlyTracking(false), mbMapUpdated(false), mbVO(false), mpORBVocabulary(pVoc), mpKeyFrameDB(pKFDB), mpInitializer(static_cast(NULL)), mpSystem(pSys), mpViewer(NULL), - mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpAtlas(pAtlas), mnLastRelocFrameId(0), time_recently_lost(3.0), + mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpAtlas(pAtlas), mnLastRelocFrameId(0), time_recently_lost(5.0), mnInitialFrameId(0), mbCreatedMap(false), mnFirstFrameId(0), mpCamera2(nullptr) { // Load camera parameters from settings file