diff --git a/modules/rapid/doc/rapid.bib b/modules/rapid/doc/rapid.bib index abb4ec87a..7176fe323 100644 --- a/modules/rapid/doc/rapid.bib +++ b/modules/rapid/doc/rapid.bib @@ -16,3 +16,14 @@ year={2002}, publisher={IEEE} } + +@article{seo2013optimal, + title={Optimal local searching for fast and robust textureless 3D object tracking in highly cluttered backgrounds}, + author={Seo, Byung-Kuk and Park, Hanhoon and Park, Jong-Il and Hinterstoisser, Stefan and Ilic, Slobodan}, + journal={IEEE transactions on visualization and computer graphics}, + volume={20}, + number={1}, + pages={99--110}, + year={2013}, + publisher={IEEE} +} diff --git a/modules/rapid/include/opencv2/rapid.hpp b/modules/rapid/include/opencv2/rapid.hpp index 5c045adca..3e2007a4b 100644 --- a/modules/rapid/include/opencv2/rapid.hpp +++ b/modules/rapid/include/opencv2/rapid.hpp @@ -122,6 +122,33 @@ CV_EXPORTS_W void convertCorrespondencies(InputArray cols, InputArray srcLocatio */ CV_EXPORTS_W float rapid(InputArray img, int num, int len, InputArray pts3d, InputArray tris, InputArray K, InputOutputArray rvec, InputOutputArray tvec, CV_OUT double* rmsd = 0); + +/// Abstract base class for stateful silhouette trackers +class CV_EXPORTS_W Tracker : public Algorithm +{ +public: + virtual ~Tracker(); + CV_WRAP virtual float + compute(InputArray img, int num, int len, InputArray K, InputOutputArray rvec, InputOutputArray tvec, + const TermCriteria& termcrit = TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS, 5, 1.5)) = 0; + CV_WRAP virtual void clearState() = 0; +}; + +/// wrapper around @ref rapid function for uniform access +class CV_EXPORTS_W Rapid : public Tracker +{ +public: + CV_WRAP static Ptr create(InputArray pts3d, InputArray tris); +}; + +/** implements "Optimal local searching for fast and robust textureless 3D object tracking in highly + * cluttered backgrounds" @cite seo2013optimal + */ +class CV_EXPORTS_W OLSTracker : public Tracker +{ +public: + CV_WRAP static Ptr create(InputArray pts3d, InputArray tris, int histBins = 8, uchar sobelThesh = 10); +}; //! @} } /* namespace rapid */ } /* namespace cv */ diff --git a/modules/rapid/src/histogram.cpp b/modules/rapid/src/histogram.cpp new file mode 100644 index 000000000..f47c99bfc --- /dev/null +++ b/modules/rapid/src/histogram.cpp @@ -0,0 +1,231 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + +#include "precomp.hpp" + +namespace cv +{ +namespace rapid +{ +static void compute1DCanny(const cv::Mat& src, cv::Mat& dst, uchar threshold) +{ + compute1DSobel(src, dst); + + // step2: compute 1D non-maximum suppression + threshold + for (int i = 0; i < dst.rows; i++) + { + for (int j = 1; j < dst.cols - 1; j++) + { + if (dst.at(i, j) <= dst.at(i, j - 1) || dst.at(i, j) <= dst.at(i, j + 1)) + dst.at(i, j) = 0; + + // threshold + if(dst.at(i, j) < threshold) + dst.at(i, j) = 0; + } + } +} + +static void calcHueSatHist(const Mat_& hsv, Mat_& hist) +{ + for (int i = 0; i < hsv.rows; i++) + { + for (int j = 0; j < hsv.cols; j++) + { + const Vec3b& c = hsv(i, j); + // thresholds as in sec. 4.1 + if (c[1] > 25 && c[2] > 50) + { + hist(c[0] * hist.rows / 256, c[1] * hist.cols / 256)++; + } + } + } +} + +static float sum(const Mat_& hist) +{ + CV_DbgAssert(hist.isContinuous()); + float ret = 0; + int N = int(hist.total()); + const float* ptr = hist.ptr(); + for (int i = 0; i < N; i++) + ret += ptr[i]; + return ret; +} + +static double bhattacharyyaCoeff(const Mat& a, const Mat& b) +{ + CV_DbgAssert(a.isContinuous() && b.isContinuous()); + int N = int(a.total()); + double ret = 0; + const float* aptr = a.ptr(); + const float* bptr = b.ptr(); + for (int i = 0; i < N; i++) + ret += std::sqrt(aptr[i] * bptr[i]); + return ret; +} + +static void findCorrespondenciesOLS(const cv::Mat_& scores, cv::Mat_& cols) +{ + cols.resize(scores.rows); + for (int i = 0; i < scores.rows; i++) + { + int pos = -1; + for (int j = scores.cols - 1; j >= 0; j--) + { + if (scores(i, j) >= 0.35) + { + pos = j; + break; + } + } + + cols(i) = pos; + } +} + +struct OLSTrackerImpl : public OLSTracker +{ + Mat vtx; + Mat tris; + + Mat_ fgHist; + Mat_ bgHist; + double tau; + uchar sobelThresh; + + OLSTrackerImpl(InputArray _pts3d, InputArray _tris, int histBins, uchar _sobelThesh) + { + CV_Assert(_tris.getMat().checkVector(3, CV_32S) > 0); + CV_Assert(_pts3d.getMat().checkVector(3, CV_32F) > 0); + vtx = _pts3d.getMat(); + tris = _tris.getMat(); + + tau = 1.0; // currently does not work as intended. effectively disable + sobelThresh = _sobelThesh; + + bgHist.create(histBins, histBins); + } + + void computeAppearanceScores(const Mat& bundleHSV, const Mat& bundleGrad, Mat_& scores) const + { + scores.resize(bundleHSV.rows); + scores = 0; + Mat_ hist(fgHist.size()); + + for (int i = 0; i < bundleHSV.rows; i++) + { + int start = 0; + for (int j = 0; j < bundleHSV.cols; j++) + { + if (bundleGrad.at(i, j)) + { + // compute the histogram between last candidate point to current candidate point + // as in eq. (4) + hist = 0; + calcHueSatHist(bundleHSV({i, i + 1}, {start, j}), hist); + hist /= std::max(sum(hist), 1.0f); + + double s = bhattacharyyaCoeff(fgHist, hist); + // handle object clutter as in eq. (5) + if((1.0 - s) > tau) + s = 1.0 - bhattacharyyaCoeff(bgHist, hist); + scores(i, j) = float(s); + start = j; + } + } + } + } + + void updateFgBgHist(const Mat_& hsv, const Mat_& cols) + { + fgHist = 0; + bgHist = 0; + + for (int i = 0; i < hsv.rows; i++) + { + int col = cols(i) < 0 ? hsv.cols / 2 + 1 : cols(i); + calcHueSatHist(hsv({i, i + 1}, {0, col}), fgHist); + calcHueSatHist(hsv({i, i + 1}, {col + 1, hsv.cols}), bgHist); + } + + fgHist /= sum(fgHist); + bgHist /= sum(bgHist); + } + + float compute(InputArray img, int num, int len, InputArray K, InputOutputArray rvec, + InputOutputArray tvec, const TermCriteria& termcrit) CV_OVERRIDE + { + CV_Assert(num >= 3); + Mat pts2d, pts3d; + + float ret = 0; + + int niter = std::max(1, termcrit.maxCount); + for(int i = 0; i < niter; i++) + { + extractControlPoints(num, len, vtx, rvec, tvec, K, img.size(), tris, pts2d, pts3d); + if (pts2d.empty()) + return 0; + + Mat lineBundle, imgLoc; + extractLineBundle(len, pts2d, img, lineBundle, imgLoc); + + Mat bundleHSV; + cvtColor(lineBundle, bundleHSV, COLOR_BGR2HSV_FULL); + + Mat_ cols(num, 1); + if(fgHist.empty()) + { + cols = len + 1; + + fgHist.create(bgHist.size()); + updateFgBgHist(bundleHSV, cols); + } + + Mat bundleGrad; + compute1DCanny(lineBundle, bundleGrad, sobelThresh); + + Mat_ scores(lineBundle.size()); + computeAppearanceScores(bundleHSV, bundleGrad, scores); + findCorrespondenciesOLS(scores, cols); + + convertCorrespondencies(cols, imgLoc, pts2d, pts3d, cols > -1); + + if (pts2d.rows < 3) + return 0; + + solvePnPRefineLM(pts3d, pts2d, K, cv::noArray(), rvec, tvec); + + updateFgBgHist(bundleHSV, cols); + + ret = float(pts2d.rows) / num; + + if(termcrit.type & TermCriteria::EPS) + { + Mat tmp; + cols.copyTo(tmp, cols > 0); + tmp -= len + 1; + double rmsd = std::sqrt(norm(tmp, NORM_L2SQR) / tmp.rows); + if(rmsd < termcrit.epsilon) + break; + } + } + + return ret; + } + + void clearState() CV_OVERRIDE + { + fgHist.release(); + } +}; + +Ptr OLSTracker::create(InputArray pts3d, InputArray tris, int histBins, uchar sobelThesh) +{ + return makePtr(pts3d, tris, histBins, sobelThesh); +} + +} // namespace rapid +} // namespace cv diff --git a/modules/rapid/src/precomp.hpp b/modules/rapid/src/precomp.hpp index b92639fb3..27ad52db5 100644 --- a/modules/rapid/src/precomp.hpp +++ b/modules/rapid/src/precomp.hpp @@ -8,4 +8,12 @@ #include #include +namespace cv +{ +namespace rapid +{ +void compute1DSobel(const Mat& src, Mat& dst); +} +} // namespace cv + #endif diff --git a/modules/rapid/src/rapid.cpp b/modules/rapid/src/rapid.cpp index d05707227..9a4cb5017 100644 --- a/modules/rapid/src/rapid.cpp +++ b/modules/rapid/src/rapid.cpp @@ -199,7 +199,7 @@ void extractLineBundle(int len, InputArray ctl2d, InputArray img, OutputArray bu INTER_NEAREST); // inter_nearest as we use integer locations } -static void compute1DSobel(const Mat& src, Mat& dst) +void compute1DSobel(const Mat& src, Mat& dst) { CV_CheckDepthEQ(src.depth(), CV_8U, "only uchar images supported"); int channels = src.channels(); @@ -359,5 +359,50 @@ float rapid(InputArray img, int num, int len, InputArray vtx, InputArray tris, I return float(pts2d.rows) / num; } +Tracker::~Tracker() {} + +struct RapidImpl : public Rapid +{ + Mat pts3d; + Mat tris; + RapidImpl(InputArray _pts3d, InputArray _tris) + { + CV_Assert(_tris.getMat().checkVector(3, CV_32S) > 0); + CV_Assert(_pts3d.getMat().checkVector(3, CV_32F) > 0); + pts3d = _pts3d.getMat(); + tris = _tris.getMat(); + } + float compute(InputArray img, int num, int len, InputArray K, InputOutputArray rvec, + InputOutputArray tvec, const TermCriteria& termcrit) CV_OVERRIDE + { + float ret = 0; + int niter = std::max(1, termcrit.maxCount); + + double rmsd; + Mat cols; + for(int i = 0; i < niter; i++) + { + ret = rapid(img, num, len, pts3d, tris, K, rvec, tvec, + termcrit.type & TermCriteria::EPS ? &rmsd : NULL); + + if((termcrit.type & TermCriteria::EPS) && rmsd < termcrit.epsilon) + { + break; + } + } + return ret; + } + + void clearState() CV_OVERRIDE + { + // nothing to do + } +}; + +Ptr Rapid::create(InputArray pts3d, InputArray tris) +{ + return makePtr(pts3d, tris); +} + } /* namespace rapid */ } /* namespace cv */ diff --git a/modules/rapid/test/test_main.cpp b/modules/rapid/test/test_main.cpp index 1fc6e501e..b3050adc8 100644 --- a/modules/rapid/test/test_main.cpp +++ b/modules/rapid/test/test_main.cpp @@ -35,8 +35,10 @@ TEST(CV_Rapid, rapid) // recover pose form different position Vec3f t_init = Vec3f(0.1f, 0, 5); - for(int i = 0; i < 2; i++) // do two iteration - rapid::rapid(img, 100, 20, vtx, tris, K, rot, t_init); + auto tracker = rapid::Rapid::create(vtx, tris); + // do two iterations + TermCriteria term(TermCriteria::MAX_ITER, 2, 0); + tracker->compute(img, 100, 20, K, rot, t_init, term); // assert that it improved from init ASSERT_LT(cv::norm(trans - t_init), 0.075);