trackerMedianFlow.cpp 17 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44
/*M///////////////////////////////////////////////////////////////////////////////////////
 //
 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
 //
 //  By downloading, copying, installing or using the software you agree to this license.
 //  If you do not agree to this license, do not download, install,
 //  copy or use the software.
 //
 //
 //                           License Agreement
 //                For Open Source Computer Vision Library
 //
 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
 // Third party copyrights are property of their respective owners.
 //
 // Redistribution and use in source and binary forms, with or without modification,
 // are permitted provided that the following conditions are met:
 //
 //   * Redistribution's of source code must retain the above copyright notice,
 //     this list of conditions and the following disclaimer.
 //
 //   * Redistribution's in binary form must reproduce the above copyright notice,
 //     this list of conditions and the following disclaimer in the documentation
 //     and/or other materials provided with the distribution.
 //
 //   * The name of the copyright holders may not be used to endorse or promote products
 //     derived from this software without specific prior written permission.
 //
 // This software is provided by the copyright holders and contributors "as is" and
 // any express or implied warranties, including, but not limited to, the implied
 // warranties of merchantability and fitness for a particular purpose are disclaimed.
 // In no event shall the Intel Corporation or contributors be liable for any direct,
 // indirect, incidental, special, exemplary, or consequential damages
 // (including, but not limited to, procurement of substitute goods or services;
 // loss of use, data, or profits; or business interruption) however caused
 // and on any theory of liability, whether in contract, strict liability,
 // or tort (including negligence or otherwise) arising in any way out of
 // the use of this software, even if advised of the possibility of such damage.
 //
 //M*/

#include "precomp.hpp"
#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc.hpp"
45
#include "tracking_utils.hpp"
46 47 48
#include <algorithm>
#include <limits.h>

49
namespace
50
{
51
using namespace cv;
52

53 54
#undef MEDIAN_FLOW_TRACKER_DEBUG_LOGS
#ifdef MEDIAN_FLOW_TRACKER_DEBUG_LOGS
Alex Leontiev's avatar
Alex Leontiev committed
55 56
#define dprintf(x) printf x
#else
57
#define dprintf(x) do{} while(false)
Alex Leontiev's avatar
Alex Leontiev committed
58 59
#endif

60 61 62 63 64 65
/*
 *  TrackerMedianFlow
 */
/*
 * TODO:
 * add "non-detected" answer in algo --> test it with 2 rects --> frame-by-frame debug in TLD --> test it!!
Vladimir's avatar
Vladimir committed
66
 * take all parameters out
67 68 69 70 71 72 73 74
 *              asessment framework
 *
 *
 * FIXME:
 * when patch is cut from image to compute NCC, there can be problem with size
 * optimize (allocation<-->reallocation)
 */

Alex Leontiev's avatar
Alex Leontiev committed
75
class TrackerMedianFlowImpl : public TrackerMedianFlow{
76
public:
77
    TrackerMedianFlowImpl(TrackerMedianFlow::Params paramsIn = TrackerMedianFlow::Params()) {params=paramsIn;isInit=false;}
78 79 80 81 82 83 84 85 86
    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<Point2f>& oldPoints,const std::vector<Point2f>& newPoints,const Rect2d& oldRect,Point2f& mD);
    float dist(Point2f p1,Point2f p2);
    std::string type2str(int type);
87
#if 0
88
    void computeStatistics(std::vector<float>& data,int size=-1);
89
#endif
90 91 92 93 94 95
    void check_FB(const std::vector<Mat>& oldImagePyr,const std::vector<Mat>& newImagePyr,
                  const std::vector<Point2f>& oldPoints,const std::vector<Point2f>& newPoints,std::vector<bool>& status);
    void check_NCC(const Mat& oldImage,const Mat& newImage,
                   const std::vector<Point2f>& oldPoints,const std::vector<Point2f>& newPoints,std::vector<bool>& status);

    TrackerMedianFlow::Params params;
96 97
};

98 99 100 101 102
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.));
103

104 105 106 107 108 109 110 111 112 113 114 115 116 117
    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;
118 119
}

120 121 122 123 124 125 126 127 128 129 130 131 132 133
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<Mat>& /*responses*/ ){}
    void modelUpdateImpl(){}
};

Alex Leontiev's avatar
Alex Leontiev committed
134
void TrackerMedianFlowImpl::read( const cv::FileNode& fn )
135
{
136
    params.read( fn );
137 138
}

Alex Leontiev's avatar
Alex Leontiev committed
139
void TrackerMedianFlowImpl::write( cv::FileStorage& fs ) const
140
{
141
    params.write( fs );
142 143
}

Alex Leontiev's avatar
Alex Leontiev committed
144
bool TrackerMedianFlowImpl::initImpl( const Mat& image, const Rect2d& boundingBox ){
145 146 147 148 149 150
    model=Ptr<TrackerMedianFlowModel>(new TrackerMedianFlowModel(params));
    ((TrackerMedianFlowModel*)static_cast<TrackerModel*>(model))->setImage(image);
    ((TrackerMedianFlowModel*)static_cast<TrackerModel*>(model))->setBoudingBox(boundingBox);
    return true;
}

Alex Leontiev's avatar
Alex Leontiev committed
151
bool TrackerMedianFlowImpl::updateImpl( const Mat& image, Rect2d& boundingBox ){
152 153 154
    Mat oldImage=((TrackerMedianFlowModel*)static_cast<TrackerModel*>(model))->getImage();

    Rect2d oldBox=((TrackerMedianFlowModel*)static_cast<TrackerModel*>(model))->getBoundingBox();
Alex Leontiev's avatar
Alex Leontiev committed
155
    if(!medianFlowImpl(oldImage,image,oldBox)){
156 157 158 159 160 161 162 163
        return false;
    }
    boundingBox=oldBox;
    ((TrackerMedianFlowModel*)static_cast<TrackerModel*>(model))->setImage(image);
    ((TrackerMedianFlowModel*)static_cast<TrackerModel*>(model))->setBoudingBox(oldBox);
    return true;
}

164 165 166 167 168 169 170 171 172 173 174 175
template<typename T>
size_t filterPointsInVectors(std::vector<T>& status, std::vector<Point2f>& vec1, std::vector<Point2f>& 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++;
    }
176

177 178
    if (first_bad_idx >= status.size())
        return first_bad_idx;
179

180 181 182 183
    for(size_t i = first_bad_idx + 1; i < status.size(); i++)
    {
        if (status[i] != goodValue)
            continue;
184

185 186 187 188 189 190 191
        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());
192
    status.erase(status.begin() + first_bad_idx, status.end());
193

194
    return first_bad_idx;
195
}
196

Alex Leontiev's avatar
Alex Leontiev committed
197
bool TrackerMedianFlowImpl::medianFlowImpl(Mat oldImage,Mat newImage,Rect2d& oldBox){
198 199 200
    std::vector<Point2f> pointsToTrackOld,pointsToTrackNew;

    Mat oldImage_gray,newImage_gray;
201 202 203 204 205 206 207 208 209
    if (oldImage.channels() != 1)
        cvtColor( oldImage, oldImage_gray, COLOR_BGR2GRAY );
    else
        oldImage.copyTo(oldImage_gray);

    if (newImage.channels() != 1)
        cvtColor( newImage, newImage_gray, COLOR_BGR2GRAY );
    else
        newImage.copyTo(newImage_gray);
210 211 212 213

    //"open ended" grid
    for(int i=0;i<params.pointsInGrid;i++){
        for(int j=0;j<params.pointsInGrid;j++){
214
            pointsToTrackOld.push_back(
Alex Leontiev's avatar
Alex Leontiev committed
215
                        Point2f((float)(oldBox.x+((1.0*oldBox.width)/params.pointsInGrid)*j+.5*oldBox.width/params.pointsInGrid),
216
                                (float)(oldBox.y+((1.0*oldBox.height)/params.pointsInGrid)*i+.5*oldBox.height/params.pointsInGrid)));
217 218 219 220 221
        }
    }

    std::vector<uchar> status(pointsToTrackOld.size());
    std::vector<float> errors(pointsToTrackOld.size());
222 223 224 225 226 227 228 229 230 231 232 233

    std::vector<Mat> oldImagePyr;
    buildOpticalFlowPyramid(oldImage_gray, oldImagePyr, params.winSize, params.maxLevel, false);

    std::vector<Mat> 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());
Alex Leontiev's avatar
Alex Leontiev committed
234
    dprintf(("\t%d after LK forward\n",(int)pointsToTrackOld.size()));
235

236 237 238 239 240 241
    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;
242 243
    }

244 245 246 247 248 249 250 251
    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<bool> filter_status(pointsToTrackOld.size(), true);
    check_FB(oldImagePyr, newImagePyr, pointsToTrackOld, pointsToTrackNew, filter_status);
    check_NCC(oldImage_gray, newImage_gray, pointsToTrackOld, pointsToTrackNew, filter_status);
252 253

    // filter
254 255 256 257 258 259
    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;
260
    }
261 262 263 264

    CV_Assert(pointsToTrackOld.size() == num_good_points_after_filtering);
    CV_Assert(pointsToTrackNew.size() == num_good_points_after_filtering);

Alex Leontiev's avatar
Alex Leontiev committed
265
    dprintf(("\t%d after LK backward\n",(int)pointsToTrackOld.size()));
266

267 268 269
    std::vector<Point2f> di(pointsToTrackOld.size());
    for(size_t i=0; i<pointsToTrackOld.size(); i++){
        di[i] = pointsToTrackNew[i]-pointsToTrackOld[i];
270
    }
271

272 273 274
    Point2f mDisplacement;
    oldBox=vote(pointsToTrackOld,pointsToTrackNew,oldBox,mDisplacement);

275 276
    std::vector<float> displacements;
    for(size_t i=0;i<di.size();i++){
277
        di[i]-=mDisplacement;
278
        displacements.push_back((float)sqrt(di[i].ddot(di[i])));
279
    }
280
    float median_displacements = tracking_internal::getMedianAndDoPartition(displacements);
281 282 283
    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"));
284 285 286 287 288 289
        return false;
    }

    return true;
}

Alex Leontiev's avatar
Alex Leontiev committed
290
Rect2d TrackerMedianFlowImpl::vote(const std::vector<Point2f>& oldPoints,const std::vector<Point2f>& newPoints,const Rect2d& oldRect,Point2f& mD){
291 292
    Rect2d newRect;
    Point2d newCenter(oldRect.x+oldRect.width/2.0,oldRect.y+oldRect.height/2.0);
293
    const size_t n=oldPoints.size();
294

295
    if (n==1) {
296 297 298 299
        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;
300 301
        mD.x = newPoints[0].x-oldPoints[0].x;
        mD.y = newPoints[0].y-oldPoints[0].y;
302 303 304
        return newRect;
    }

305 306 307
    float xshift=0,yshift=0;
    std::vector<float> buf_for_location(n, 0.);
    for(size_t i=0;i<n;i++){  buf_for_location[i]=newPoints[i].x-oldPoints[i].x;  }
308
    xshift=tracking_internal::getMedianAndDoPartition(buf_for_location);
309
    newCenter.x+=xshift;
310
    for(size_t i=0;i<n;i++){  buf_for_location[i]=newPoints[i].y-oldPoints[i].y;  }
311
    yshift=tracking_internal::getMedianAndDoPartition(buf_for_location);
312
    newCenter.y+=yshift;
Alex Leontiev's avatar
Alex Leontiev committed
313
    mD=Point2f((float)xshift,(float)yshift);
314

315 316 317 318 319 320
    std::vector<double> buf_for_scale(n*(n-1)/2, 0.0);
    for(size_t i=0,ctr=0;i<n;i++){
        for(size_t j=0;j<i;j++){
            double nd=norm(newPoints[i] - newPoints[j]);
            double od=norm(oldPoints[i] - oldPoints[j]);
            buf_for_scale[ctr]=(od==0.0)?0.0:(nd/od);
321 322 323 324
            ctr++;
        }
    }

325
    double scale=tracking_internal::getMedianAndDoPartition(buf_for_scale);
326
    dprintf(("xshift, yshift, scale = %f %f %f\n",xshift,yshift,scale));
327 328 329 330
    newRect.x=newCenter.x-scale*oldRect.width/2.0;
    newRect.y=newCenter.y-scale*oldRect.height/2.0;
    newRect.width=scale*oldRect.width;
    newRect.height=scale*oldRect.height;
Alex Leontiev's avatar
Alex Leontiev committed
331 332
    dprintf(("rect old [%f %f %f %f]\n",oldRect.x,oldRect.y,oldRect.width,oldRect.height));
    dprintf(("rect [%f %f %f %f]\n",newRect.x,newRect.y,newRect.width,newRect.height));
333 334 335

    return newRect;
}
336
#if 0
Alex Leontiev's avatar
Alex Leontiev committed
337
void TrackerMedianFlowImpl::computeStatistics(std::vector<float>& data,int size){
338 339
    int binnum=10;
    if(size==-1){
Alex Leontiev's avatar
Alex Leontiev committed
340
        size=(int)data.size();
341 342 343 344 345 346 347
    }
    float mini=*std::min_element(data.begin(),data.begin()+size),maxi=*std::max_element(data.begin(),data.begin()+size);
    std::vector<int> bins(binnum,(int)0);
    for(int i=0;i<size;i++){
        bins[std::min((int)(binnum*(data[i]-mini)/(maxi-mini)),binnum-1)]++;
    }
    for(int i=0;i<binnum;i++){
Alex Leontiev's avatar
Alex Leontiev committed
348
        dprintf(("[%4f,%4f] -- %4d\n",mini+(maxi-mini)/binnum*i,mini+(maxi-mini)/binnum*(i+1),bins[i]));
349 350
    }
}
351
#endif
352 353 354 355
void TrackerMedianFlowImpl::check_FB(const std::vector<Mat>& oldImagePyr, const std::vector<Mat>& newImagePyr,
                                     const std::vector<Point2f>& oldPoints, const std::vector<Point2f>& newPoints, std::vector<bool>& status){

    if(status.empty()) {
356 357 358 359 360
        status=std::vector<bool>(oldPoints.size(),true);
    }

    std::vector<uchar> LKstatus(oldPoints.size());
    std::vector<float> errors(oldPoints.size());
361
    std::vector<float> FBerror(oldPoints.size());
362
    std::vector<Point2f> pointsToTrackReprojection;
363 364
    calcOpticalFlowPyrLK(newImagePyr, oldImagePyr,newPoints,pointsToTrackReprojection,LKstatus,errors,
                         params.winSize, params.maxLevel, params.termCriteria, 0);
365

366 367
    for(size_t i=0;i<oldPoints.size();i++){
        FBerror[i]=(float)norm(oldPoints[i]-pointsToTrackReprojection[i]);
368
    }
369
    float FBerrorMedian=tracking_internal::getMedian(FBerror);
Alex Leontiev's avatar
Alex Leontiev committed
370 371
    dprintf(("point median=%f\n",FBerrorMedian));
    dprintf(("FBerrorMedian=%f\n",FBerrorMedian));
372 373
    for(size_t i=0;i<oldPoints.size();i++){
        status[i]=status[i] && (FBerror[i] <= FBerrorMedian);
374 375
    }
}
Alex Leontiev's avatar
Alex Leontiev committed
376
void TrackerMedianFlowImpl::check_NCC(const Mat& oldImage,const Mat& newImage,
377
                                      const std::vector<Point2f>& oldPoints,const std::vector<Point2f>& newPoints,std::vector<bool>& status){
378 379 380 381

    std::vector<float> NCC(oldPoints.size(),0.0);
    Mat p1,p2;

382 383 384
    for (size_t i = 0; i < oldPoints.size(); i++) {
        p1 = getPatch(oldImage, params.winSizeNCC, oldPoints[i]);
        p2 = getPatch(newImage, params.winSizeNCC, newPoints[i]);
385

386
        NCC[i] = (float)tracking_internal::computeNCC(p1, p2);
387
    }
388
    float median = tracking_internal::getMedian(NCC);
389 390 391
    for(size_t i = 0; i < oldPoints.size(); i++) {
        status[i] = status[i] && (NCC[i] >= median);
    }
392
}
393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444

} /* 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;
}

445
Ptr<TrackerMedianFlow> TrackerMedianFlow::create(const TrackerMedianFlow::Params &parameters){
446 447
    return Ptr<TrackerMedianFlowImpl>(new TrackerMedianFlowImpl(parameters));
}
448 449 450
Ptr<TrackerMedianFlow> TrackerMedianFlow::create(){
    return Ptr<TrackerMedianFlowImpl>(new TrackerMedianFlowImpl());
}
451

452
} /* namespace cv */