diff --git a/modules/tracking/include/opencv2/tracking/tracker.hpp b/modules/tracking/include/opencv2/tracking/tracker.hpp index 3b32033f9..88301d254 100644 --- a/modules/tracking/include/opencv2/tracking/tracker.hpp +++ b/modules/tracking/include/opencv2/tracking/tracker.hpp @@ -1156,9 +1156,16 @@ class CV_EXPORTS TrackerMedianFlow : public Tracker public: struct CV_EXPORTS Params { - Params(); - int pointsInGrid; //! #include -namespace cv +namespace { +using namespace cv; -#undef ALEX_DEBUG -#ifdef ALEX_DEBUG -#define dfprintf(x) fprintf x +#undef MEDIAN_FLOW_TRACKER_DEBUG_LOGS +#ifdef MEDIAN_FLOW_TRACKER_DEBUG_LOGS #define dprintf(x) printf x #else -#define dfprintf(x) -#define dprintf(x) +#define dprintf(x) do{} while(false) #endif /* @@ -70,78 +69,79 @@ namespace cv * FIXME: * when patch is cut from image to compute NCC, there can be problem with size * optimize (allocation<-->reallocation) - * optimize (remove vector.erase() calls) - * bring "out" all the parameters to TrackerMedianFlow::Param */ class TrackerMedianFlowImpl : public TrackerMedianFlow{ - public: - TrackerMedianFlowImpl(TrackerMedianFlow::Params paramsIn):termcrit(TermCriteria::COUNT|TermCriteria::EPS,20,0.3){params=paramsIn;isInit=false;} - void read( const FileNode& fn ); - void write( FileStorage& fs ) const; - private: - bool initImpl( const Mat& image, const Rect2d& boundingBox ); - bool updateImpl( const Mat& image, Rect2d& boundingBox ); - bool medianFlowImpl(Mat oldImage,Mat newImage,Rect2d& oldBox); - Rect2d vote(const std::vector& oldPoints,const std::vector& newPoints,const Rect2d& oldRect,Point2f& mD); - //FIXME: this can be optimized: current method uses sort->select approach, there are O(n) selection algo for median; besides - //it makes copy all the time - template - T getMedian( std::vector& values,int size=-1); - float dist(Point2f p1,Point2f p2); - std::string type2str(int type); - void computeStatistics(std::vector& data,int size=-1); - void check_FB(const Mat& oldImage,const Mat& newImage, - const std::vector& oldPoints,const std::vector& newPoints,std::vector& status); - void check_NCC(const Mat& oldImage,const Mat& newImage, - const std::vector& oldPoints,const std::vector& newPoints,std::vector& status); - inline double l2distance(Point2f p1,Point2f p2); - - TrackerMedianFlow::Params params; - TermCriteria termcrit; +public: + TrackerMedianFlowImpl(TrackerMedianFlow::Params paramsIn) {params=paramsIn;isInit=false;} + void read( const FileNode& fn ); + void write( FileStorage& fs ) const; +private: + bool initImpl( const Mat& image, const Rect2d& boundingBox ); + bool updateImpl( const Mat& image, Rect2d& boundingBox ); + bool medianFlowImpl(Mat oldImage,Mat newImage,Rect2d& oldBox); + Rect2d vote(const std::vector& oldPoints,const std::vector& newPoints,const Rect2d& oldRect,Point2f& mD); + float dist(Point2f p1,Point2f p2); + std::string type2str(int type); + void computeStatistics(std::vector& data,int size=-1); + void check_FB(const std::vector& oldImagePyr,const std::vector& newImagePyr, + const std::vector& oldPoints,const std::vector& newPoints,std::vector& status); + void check_NCC(const Mat& oldImage,const Mat& newImage, + const std::vector& oldPoints,const std::vector& newPoints,std::vector& status); + + TrackerMedianFlow::Params params; }; -class TrackerMedianFlowModel : public TrackerModel{ - public: - TrackerMedianFlowModel(TrackerMedianFlow::Params /*params*/){} - Rect2d getBoundingBox(){return boundingBox_;} - void setBoudingBox(Rect2d boundingBox){boundingBox_=boundingBox;} - Mat getImage(){return image_;} - void setImage(const Mat& image){image.copyTo(image_);} - protected: - Rect2d boundingBox_; - Mat image_; - void modelEstimationImpl( const std::vector& /*responses*/ ){} - void modelUpdateImpl(){} -}; +template +T getMedian( const std::vector& values ); -/* - * Parameters - */ -TrackerMedianFlow::Params::Params(){ - pointsInGrid=10; -} +template +T getMedianAndDoPartition( std::vector& values ); -void TrackerMedianFlow::Params::read( const cv::FileNode& fn ){ - pointsInGrid=fn["pointsInGrid"]; -} +Mat getPatch(Mat image, Size patch_size, Point2f patch_center) +{ + Mat patch; + Point2i roi_strat_corner(cvRound(patch_center.x - patch_size.width / 2.), + cvRound(patch_center.y - patch_size.height / 2.)); -void TrackerMedianFlow::Params::write( cv::FileStorage& fs ) const{ - fs << "pointsInGrid" << pointsInGrid; + Rect2i patch_rect(roi_strat_corner, patch_size); + + if(patch_rect == (patch_rect & Rect2i(0, 0, image.cols, image.rows))) + { + patch = image(patch_rect); + } + else + { + getRectSubPix(image, patch_size, + Point2f((float)(patch_rect.x + patch_size.width / 2.), + (float)(patch_rect.y + patch_size.height / 2.)), patch); + } + + return patch; } +class TrackerMedianFlowModel : public TrackerModel{ +public: + TrackerMedianFlowModel(TrackerMedianFlow::Params /*params*/){} + Rect2d getBoundingBox(){return boundingBox_;} + void setBoudingBox(Rect2d boundingBox){boundingBox_=boundingBox;} + Mat getImage(){return image_;} + void setImage(const Mat& image){image.copyTo(image_);} +protected: + Rect2d boundingBox_; + Mat image_; + void modelEstimationImpl( const std::vector& /*responses*/ ){} + void modelUpdateImpl(){} +}; + void TrackerMedianFlowImpl::read( const cv::FileNode& fn ) { - params.read( fn ); + params.read( fn ); } void TrackerMedianFlowImpl::write( cv::FileStorage& fs ) const { - params.write( fs ); -} - -Ptr TrackerMedianFlow::createTracker(const TrackerMedianFlow::Params ¶meters){ - return Ptr(new TrackerMedianFlowImpl(parameters)); + params.write( fs ); } bool TrackerMedianFlowImpl::initImpl( const Mat& image, const Rect2d& boundingBox ){ @@ -164,28 +164,38 @@ bool TrackerMedianFlowImpl::updateImpl( const Mat& image, Rect2d& boundingBox ){ return true; } -std::string TrackerMedianFlowImpl::type2str(int type) { - std::string r; +template +size_t filterPointsInVectors(std::vector& status, std::vector& vec1, std::vector& vec2, T goodValue) +{ + CV_DbgAssert(status.size() == vec1.size() && status.size() == vec2.size()); + + size_t first_bad_idx = 0; + while(first_bad_idx < status.size()) + { + if(status[first_bad_idx] != goodValue) + break; + first_bad_idx++; + } - uchar depth = type & CV_MAT_DEPTH_MASK; - uchar chans = (uchar)(1 + (type >> CV_CN_SHIFT)); + if (first_bad_idx >= status.size()) + return first_bad_idx; - switch ( depth ) { - case CV_8U: r = "8U"; break; - case CV_8S: r = "8S"; break; - case CV_16U: r = "16U"; break; - case CV_16S: r = "16S"; break; - case CV_32S: r = "32S"; break; - case CV_32F: r = "32F"; break; - case CV_64F: r = "64F"; break; - default: r = "User"; break; - } + for(size_t i = first_bad_idx + 1; i < status.size(); i++) + { + if (status[i] != goodValue) + continue; - r += "C"; - r += (chans+'0'); + status[first_bad_idx] = goodValue; + vec1[first_bad_idx] = vec1[i]; + vec2[first_bad_idx] = vec2[i]; + first_bad_idx++; + } + vec1.erase(vec1.begin() + first_bad_idx, vec1.end()); + vec2.erase(vec2.begin() + first_bad_idx, vec2.end()); - return r; + return first_bad_idx; } + bool TrackerMedianFlowImpl::medianFlowImpl(Mat oldImage,Mat newImage,Rect2d& oldBox){ std::vector pointsToTrackOld,pointsToTrackNew; @@ -203,51 +213,76 @@ bool TrackerMedianFlowImpl::medianFlowImpl(Mat oldImage,Mat newImage,Rect2d& old //"open ended" grid for(int i=0;i status(pointsToTrackOld.size()); std::vector errors(pointsToTrackOld.size()); - calcOpticalFlowPyrLK(oldImage_gray, newImage_gray,pointsToTrackOld,pointsToTrackNew,status,errors,Size(3,3),5,termcrit,0); + + std::vector oldImagePyr; + buildOpticalFlowPyramid(oldImage_gray, oldImagePyr, params.winSize, params.maxLevel, false); + + std::vector newImagePyr; + buildOpticalFlowPyramid(newImage_gray, newImagePyr, params.winSize, params.maxLevel, false); + + calcOpticalFlowPyrLK(oldImagePyr,newImagePyr,pointsToTrackOld,pointsToTrackNew,status,errors, + params.winSize, params.maxLevel, params.termCriteria, 0); + + CV_Assert(pointsToTrackNew.size() == pointsToTrackOld.size()); + CV_Assert(status.size() == pointsToTrackOld.size()); dprintf(("\t%d after LK forward\n",(int)pointsToTrackOld.size())); - std::vector di; - for(int i=0;i<(int)pointsToTrackOld.size();i++){ - if(status[i]==1){ - di.push_back(pointsToTrackNew[i]-pointsToTrackOld[i]); - } + size_t num_good_points_after_optical_flow = filterPointsInVectors(status, pointsToTrackOld, pointsToTrackNew, (uchar)1); + + dprintf(("\t num_good_points_after_optical_flow = %d\n",num_good_points_after_optical_flow)); + + if (num_good_points_after_optical_flow == 0) { + return false; } - std::vector filter_status; - check_FB(oldImage_gray,newImage_gray,pointsToTrackOld,pointsToTrackNew,filter_status); - check_NCC(oldImage_gray,newImage_gray,pointsToTrackOld,pointsToTrackNew,filter_status); + CV_Assert(pointsToTrackOld.size() == num_good_points_after_optical_flow); + CV_Assert(pointsToTrackNew.size() == num_good_points_after_optical_flow); + + dprintf(("\t%d after LK forward after removing points with bad status\n",(int)pointsToTrackOld.size())); + + std::vector filter_status(pointsToTrackOld.size(), true); + check_FB(oldImagePyr, newImagePyr, pointsToTrackOld, pointsToTrackNew, filter_status); + check_NCC(oldImage_gray, newImage_gray, pointsToTrackOld, pointsToTrackNew, filter_status); // filter - for(int i=0;i<(int)pointsToTrackOld.size();i++){ - if(!filter_status[i]){ - pointsToTrackOld.erase(pointsToTrackOld.begin()+i); - pointsToTrackNew.erase(pointsToTrackNew.begin()+i); - filter_status.erase(filter_status.begin()+i); - i--; - } + size_t num_good_points_after_filtering = filterPointsInVectors(filter_status, pointsToTrackOld, pointsToTrackNew, true); + + dprintf(("\t num_good_points_after_filtering = %d\n",num_good_points_after_filtering)); + + if(num_good_points_after_filtering == 0){ + return false; } + + CV_Assert(pointsToTrackOld.size() == num_good_points_after_filtering); + CV_Assert(pointsToTrackNew.size() == num_good_points_after_filtering); + dprintf(("\t%d after LK backward\n",(int)pointsToTrackOld.size())); - if(pointsToTrackOld.size()==0 || di.size()==0){ - return false; + std::vector di(pointsToTrackOld.size()); + for(size_t i=0; i displacements; - for(int i=0;i<(int)di.size();i++){ + std::vector displacements; + for(size_t i=0;i10){ + float median_displacements = getMedianAndDoPartition(displacements); + dprintf(("\tmedian of length of difference of displacements = %f\n", median_displacements)); + if(median_displacements > params.maxMedianLengthOfDisplacementDifference){ + dprintf(("\tmedian flow tracker returns false due to big median length of difference between displacements\n")); return false; } @@ -255,77 +290,52 @@ bool TrackerMedianFlowImpl::medianFlowImpl(Mat oldImage,Mat newImage,Rect2d& old } Rect2d TrackerMedianFlowImpl::vote(const std::vector& oldPoints,const std::vector& newPoints,const Rect2d& oldRect,Point2f& mD){ - static int iteration=0;//FIXME -- we don't want this static var in final release Rect2d newRect; Point2d newCenter(oldRect.x+oldRect.width/2.0,oldRect.y+oldRect.height/2.0); - int n=(int)oldPoints.size(); - std::vector buf(std::max(n*(n-1)/2,3),0.0); + const size_t n=oldPoints.size(); - if(oldPoints.size()==1){ + if (n==1) { newRect.x=oldRect.x+newPoints[0].x-oldPoints[0].x; newRect.y=oldRect.y+newPoints[0].y-oldPoints[0].y; newRect.width=oldRect.width; newRect.height=oldRect.height; + mD.x = newPoints[0].x-oldPoints[0].x; + mD.y = newPoints[0].y-oldPoints[0].y; return newRect; } - double xshift=0,yshift=0; - for(int i=0;i buf_for_location(n, 0.); + for(size_t i=0;i buf_for_scale(n*(n-1)/2, 0.0); + for(size_t i=0,ctr=0;i -T TrackerMedianFlowImpl::getMedian(std::vector& values,int size){ - if(size==-1){ - size=(int)values.size(); - } - std::vector copy(values.begin(),values.begin()+size); - std::sort(copy.begin(),copy.end()); - if(size%2==0){ - return (copy[size/2-1]+copy[size/2])/((T)2.0); - }else{ - return copy[(size-1)/2]; - } -} - void TrackerMedianFlowImpl::computeStatistics(std::vector& data,int size){ int binnum=10; if(size==-1){ @@ -340,56 +350,139 @@ void TrackerMedianFlowImpl::computeStatistics(std::vector& data,int size) dprintf(("[%4f,%4f] -- %4d\n",mini+(maxi-mini)/binnum*i,mini+(maxi-mini)/binnum*(i+1),bins[i])); } } -double TrackerMedianFlowImpl::l2distance(Point2f p1,Point2f p2){ - double dx=p1.x-p2.x, dy=p1.y-p2.y; - return sqrt(dx*dx+dy*dy); -} -void TrackerMedianFlowImpl::check_FB(const Mat& oldImage,const Mat& newImage, - const std::vector& oldPoints,const std::vector& newPoints,std::vector& status){ - if(status.size()==0){ +void TrackerMedianFlowImpl::check_FB(const std::vector& oldImagePyr, const std::vector& newImagePyr, + const std::vector& oldPoints, const std::vector& newPoints, std::vector& status){ + + if(status.empty()) { status=std::vector(oldPoints.size(),true); } std::vector LKstatus(oldPoints.size()); std::vector errors(oldPoints.size()); - std::vector FBerror(oldPoints.size()); + std::vector FBerror(oldPoints.size()); std::vector pointsToTrackReprojection; - calcOpticalFlowPyrLK(newImage, oldImage,newPoints,pointsToTrackReprojection,LKstatus,errors,Size(3,3),5,termcrit,0); + calcOpticalFlowPyrLK(newImagePyr, oldImagePyr,newPoints,pointsToTrackReprojection,LKstatus,errors, + params.winSize, params.maxLevel, params.termCriteria, 0); - for(int i=0;i<(int)oldPoints.size();i++){ - FBerror[i]=l2distance(oldPoints[i],pointsToTrackReprojection[i]); + for(size_t i=0;i& oldPoints,const std::vector& newPoints,std::vector& status){ + const std::vector& oldPoints,const std::vector& newPoints,std::vector& status){ std::vector NCC(oldPoints.size(),0.0); - Size patch(30,30); Mat p1,p2; - for (int i = 0; i < (int)oldPoints.size(); i++) { - getRectSubPix( oldImage, patch, oldPoints[i],p1); - getRectSubPix( newImage, patch, newPoints[i],p2); + for (size_t i = 0; i < oldPoints.size(); i++) { + p1 = getPatch(oldImage, params.winSizeNCC, oldPoints[i]); + p2 = getPatch(newImage, params.winSizeNCC, newPoints[i]); - const int N=900; + const int patch_area=params.winSizeNCC.area(); double s1=sum(p1)(0),s2=sum(p2)(0); double n1=norm(p1),n2=norm(p2); double prod=p1.dot(p2); - double sq1=sqrt(n1*n1-s1*s1/N),sq2=sqrt(n2*n2-s2*s2/N); - double ares=(sq2==0)?sq1/abs(sq1):(prod-s1*s2/N)/sq1/sq2; - - NCC[i] = (float)ares; - } - float median = getMedian(NCC); - for(int i = 0; i < (int)oldPoints.size(); i++) { - status[i] = status[i] && (NCC[i]>median); - } + double sq1=sqrt(n1*n1-s1*s1/patch_area),sq2=sqrt(n2*n2-s2*s2/patch_area); + double ares=(sq2==0)?sq1/abs(sq1):(prod-s1*s2/patch_area)/sq1/sq2; + + NCC[i] = (float)ares; + } + float median = getMedian(NCC); + for(size_t i = 0; i < oldPoints.size(); i++) { + status[i] = status[i] && (NCC[i] >= median); + } } + +template +T getMedian(const std::vector& values) +{ + std::vector copy(values); + return getMedianAndDoPartition(copy); +} + +template +T getMedianAndDoPartition(std::vector& values) +{ + size_t size = values.size(); + if(size%2==0) + { + std::nth_element(values.begin(), values.begin() + size/2-1, values.end()); + T firstMedian = values[size/2-1]; + + std::nth_element(values.begin(), values.begin() + size/2, values.end()); + T secondMedian = values[size/2]; + + return (firstMedian + secondMedian) / (T)2; + } + else + { + size_t medianIndex = (size - 1) / 2; + std::nth_element(values.begin(), values.begin() + medianIndex, values.end()); + + return values[medianIndex]; + } +} + +} /* anonymous namespace */ + +namespace cv +{ +/* + * Parameters + */ +TrackerMedianFlow::Params::Params() { + pointsInGrid=10; + winSize = Size(3,3); + maxLevel = 5; + termCriteria = TermCriteria(TermCriteria::COUNT|TermCriteria::EPS,20,0.3); + winSizeNCC = Size(30,30); + maxMedianLengthOfDisplacementDifference = 10; +} + +void TrackerMedianFlow::Params::read( const cv::FileNode& fn ){ + *this = TrackerMedianFlow::Params(); + + if (!fn["winSize"].empty()) + fn["winSize"] >> winSize; + + if(!fn["winSizeNCC"].empty()) + fn["winSizeNCC"] >> winSizeNCC; + + if(!fn["pointsInGrid"].empty()) + fn["pointsInGrid"] >> pointsInGrid; + + if(!fn["maxLevel"].empty()) + fn["maxLevel"] >> maxLevel; + + if(!fn["maxMedianLengthOfDisplacementDifference"].empty()) + fn["maxMedianLengthOfDisplacementDifference"] >> maxMedianLengthOfDisplacementDifference; + + if(!fn["termCriteria_maxCount"].empty()) + fn["termCriteria_maxCount"] >> termCriteria.maxCount; + + if(!fn["termCriteria_epsilon"].empty()) + fn["termCriteria_epsilon"] >> termCriteria.epsilon; +} + +void TrackerMedianFlow::Params::write( cv::FileStorage& fs ) const{ + fs << "pointsInGrid" << pointsInGrid; + fs << "winSize" << winSize; + fs << "maxLevel" << maxLevel; + fs << "termCriteria_maxCount" << termCriteria.maxCount; + fs << "termCriteria_epsilon" << termCriteria.epsilon; + fs << "winSizeNCC" << winSizeNCC; + fs << "maxMedianLengthOfDisplacementDifference" << maxMedianLengthOfDisplacementDifference; +} + +Ptr TrackerMedianFlow::createTracker(const TrackerMedianFlow::Params ¶meters){ + return Ptr(new TrackerMedianFlowImpl(parameters)); +} + } /* namespace cv */ diff --git a/modules/tracking/test/test_trackerParametersIO.cpp b/modules/tracking/test/test_trackerParametersIO.cpp new file mode 100644 index 000000000..beda09c40 --- /dev/null +++ b/modules/tracking/test/test_trackerParametersIO.cpp @@ -0,0 +1,60 @@ +// 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 "test_precomp.hpp" +#include "opencv2/tracking.hpp" + +using namespace cv; + +TEST(MEDIAN_FLOW_Parameters_IO, MEDIAN_FLOW) +{ + TrackerMedianFlow::Params parameters; + + parameters.maxLevel = 10; + parameters.maxMedianLengthOfDisplacementDifference = 11; + parameters.pointsInGrid = 12; + parameters.winSize = Size(6, 5); + parameters.winSizeNCC = Size(41, 40); + parameters.termCriteria.maxCount = 100; + parameters.termCriteria.epsilon = 0.1; + + FileStorage fsWriter("parameters.xml", FileStorage::WRITE + FileStorage::MEMORY); + parameters.write(fsWriter); + + String serializedParameters = fsWriter.releaseAndGetString(); + + FileStorage fsReader(serializedParameters, FileStorage::READ + FileStorage::MEMORY); + + TrackerMedianFlow::Params readParameters; + readParameters.read(fsReader.root()); + + ASSERT_EQ(parameters.maxLevel, readParameters.maxLevel); + ASSERT_EQ(parameters.maxMedianLengthOfDisplacementDifference, + readParameters.maxMedianLengthOfDisplacementDifference); + ASSERT_EQ(parameters.pointsInGrid, readParameters.pointsInGrid); + ASSERT_EQ(parameters.winSize, readParameters.winSize); + ASSERT_EQ(parameters.winSizeNCC, readParameters.winSizeNCC); + ASSERT_EQ(parameters.termCriteria.epsilon, readParameters.termCriteria.epsilon); + ASSERT_EQ(parameters.termCriteria.maxCount, readParameters.termCriteria.maxCount); +} + + +TEST(MEDIAN_FLOW_Parameters_IO_Default_Value_If_Absent, MEDIAN_FLOW) +{ + TrackerMedianFlow::Params defaultParameters; + + FileStorage fsReader(String("%YAML 1.0"), FileStorage::READ + FileStorage::MEMORY); + + TrackerMedianFlow::Params readParameters; + readParameters.read(fsReader.root()); + + ASSERT_EQ(defaultParameters.maxLevel, readParameters.maxLevel); + ASSERT_EQ(defaultParameters.maxMedianLengthOfDisplacementDifference, + readParameters.maxMedianLengthOfDisplacementDifference); + ASSERT_EQ(defaultParameters.pointsInGrid, readParameters.pointsInGrid); + ASSERT_EQ(defaultParameters.winSize, readParameters.winSize); + ASSERT_EQ(defaultParameters.winSizeNCC, readParameters.winSizeNCC); + ASSERT_EQ(defaultParameters.termCriteria.epsilon, readParameters.termCriteria.epsilon); + ASSERT_EQ(defaultParameters.termCriteria.maxCount, readParameters.termCriteria.maxCount); +}