opencv/tests/cv/src/optflow.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////\r
   2 //\r
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
   4 //\r
   5 //  By downloading, copying, installing or using the software you agree to this license.\r
   6 //  If you do not agree to this license, do not download, install,\r
   7 //  copy or use the software.\r
   8 //\r
   9 //\r
  10 //                           License Agreement\r
  11 //                For Open Source Computer Vision Library\r
  12 //\r
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.\r
  14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.\r
  15 // Third party copyrights are property of their respective owners.\r
  16 //\r
  17 // Redistribution and use in source and binary forms, with or without modification,\r
  18 // are permitted provided that the following conditions are met:\r
  19 //\r
  20 //   * Redistribution's of source code must retain the above copyright notice,\r
  21 //     this list of conditions and the following disclaimer.\r
  22 //\r
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,\r
  24 //     this list of conditions and the following disclaimer in the documentation\r
  25 //     and/or other materials provided with the distribution.\r
  26 //\r
  27 //   * The name of the copyright holders may not be used to endorse or promote products\r
  28 //     derived from this software without specific prior written permission.\r
  29 //\r
  30 // This software is provided by the copyright holders and contributors "as is" and\r
  31 // any express or implied warranties, including, but not limited to, the implied\r
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.\r
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,\r
  34 // indirect, incidental, special, exemplary, or consequential damages\r
  35 // (including, but not limited to, procurement of substitute goods or services;\r
  36 // loss of use, data, or profits; or business interruption) however caused\r
  37 // and on any theory of liability, whether in contract, strict liability,\r
  38 // or tort (including negligence or otherwise) arising in any way out of\r
  39 // the use of this software, even if advised of the possibility of such damage.\r
  40 //\r
  41 //M*/\r
  42 \r
  43 #include "cvtest.h"\r
  44 #include <string>\r
  45 #include <iostream>\r
  46 #include <fstream>\r
  47 #include <iterator>\r
  48 #include <limits>\r
  49 #include "cvaux.h"\r
  50 \r
  51 using namespace cv;\r
  52 using namespace std;\r
  53 \r
  54 class CV_OptFlowTest : public CvTest\r
  55 {\r
  56 public:\r
  57     CV_OptFlowTest();\r
  58     ~CV_OptFlowTest();    \r
  59 protected:    \r
  60     void run(int);\r
  61 \r
  62     bool runDense(const Point& shift = Point(3, 0));    \r
  63     bool runSparse();\r
  64 };\r
  65 \r
  66 CV_OptFlowTest::CV_OptFlowTest(): CvTest( "algorithm-opticalflow", "?" )\r
  67 {\r
  68     support_testing_modes = CvTS::CORRECTNESS_CHECK_MODE;\r
  69 }\r
  70 CV_OptFlowTest::~CV_OptFlowTest() {}\r
  71 \r
  72 \r
  73 Mat copnvert2flow(const Mat& velx, const Mat& vely)\r
  74 {\r
  75     Mat flow(velx.size(), CV_32FC2);\r
  76     for(int y = 0 ; y < flow.rows; ++y)\r
  77         for(int x = 0 ; x < flow.cols; ++x)                        \r
  78             flow.at<Point2f>(y, x) = Point2f(velx.at<float>(y, x), vely.at<float>(y, x));            \r
  79     return flow;\r
  80 }\r
  81 \r
  82 void calcOpticalFlowLK( const Mat& prev, const Mat& curr, Size winSize, Mat& flow )\r
  83 {\r
  84     Mat velx(prev.size(), CV_32F), vely(prev.size(), CV_32F); \r
  85     CvMat cvvelx = velx;    CvMat cvvely = vely;\r
  86     CvMat cvprev = prev;    CvMat cvcurr = curr;\r
  87     cvCalcOpticalFlowLK( &cvprev, &cvcurr, winSize, &cvvelx, &cvvely );\r
  88     flow = copnvert2flow(velx, vely);\r
  89 }\r
  90 \r
  91 void calcOpticalFlowBM( const Mat& prev, const Mat& curr, Size bSize, Size shiftSize, Size maxRange, int usePrevious, Mat& flow )\r
  92 {\r
  93     Size sz((curr.cols - bSize.width)/shiftSize.width, (curr.rows - bSize.height)/shiftSize.height);\r
  94     Mat velx(sz, CV_32F), vely(sz, CV_32F);    \r
  95 \r
  96     CvMat cvvelx = velx;    CvMat cvvely = vely;\r
  97     CvMat cvprev = prev;    CvMat cvcurr = curr;\r
  98     cvCalcOpticalFlowBM( &cvprev, &cvcurr, bSize, shiftSize, maxRange, usePrevious, &cvvelx, &cvvely);                     \r
  99     flow = copnvert2flow(velx, vely);\r
 100 }\r
 101 \r
 102 void calcOpticalFlowHS( const Mat& prev, const Mat& curr, int usePrevious, double lambda, TermCriteria criteria, Mat& flow)\r
 103 {        \r
 104     Mat velx(prev.size(), CV_32F), vely(prev.size(), CV_32F);\r
 105     CvMat cvvelx = velx;    CvMat cvvely = vely;\r
 106     CvMat cvprev = prev;    CvMat cvcurr = curr;\r
 107     cvCalcOpticalFlowHS( &cvprev, &cvcurr, usePrevious, &cvvelx, &cvvely, lambda, criteria );\r
 108     flow = copnvert2flow(velx, vely);\r
 109 }\r
 110 \r
 111 void calcAffineFlowPyrLK( const Mat& prev, const Mat& curr, \r
 112                           const vector<Point2f>& prev_features, vector<Point2f>& curr_features,\r
 113                           vector<uchar>& status, vector<float>& track_error, vector<float>& matrices, \r
 114                           TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,30, 0.01), \r
 115                           Size win_size = Size(15, 15), int level = 3, int flags = 0)\r
 116 {\r
 117     CvMat cvprev = prev;\r
 118     CvMat cvcurr = curr;\r
 119 \r
 120     size_t count = prev_features.size();\r
 121     curr_features.resize(count);\r
 122     status.resize(count);\r
 123     track_error.resize(count);\r
 124     matrices.resize(count * 6);\r
 125 \r
 126     cvCalcAffineFlowPyrLK( &cvprev, &cvcurr, 0, 0, \r
 127         (const CvPoint2D32f*)&prev_features[0], (CvPoint2D32f*)&curr_features[0], &matrices[0], \r
 128         (int)count, win_size, level, (char*)&status[0], &track_error[0], criteria, flags );\r
 129 }\r
 130 \r
 131 double showFlowAndCalcError(const string& name, const Mat& gray, const Mat& flow, \r
 132                             const Rect& where, const Point& d, \r
 133                             bool showImages = false, bool writeError = false)\r
 134 {       \r
 135     const int mult = 16;\r
 136 \r
 137     if (showImages)\r
 138     {\r
 139         Mat tmp, cflow;    \r
 140         resize(gray, tmp, gray.size() * mult, 0, 0, INTER_NEAREST);            \r
 141         cvtColor(tmp, cflow, CV_GRAY2BGR);        \r
 142 \r
 143         const float m2 = 0.3f;   \r
 144         const float minVel = 0.1f;\r
 145 \r
 146         for(int y = 0; y < flow.rows; ++y)\r
 147             for(int x = 0; x < flow.cols; ++x)\r
 148             {\r
 149                 Point2f f = flow.at<Point2f>(y, x);                          \r
 150 \r
 151                 if (f.x * f.x + f.y * f.y > minVel * minVel)\r
 152                 {\r
 153                     Point p1 = Point(x, y) * mult;\r
 154                     Point p2 = Point(cvRound((x + f.x*m2) * mult), cvRound((y + f.y*m2) * mult));\r
 155 \r
 156                     line(cflow, p1, p2, CV_RGB(0, 255, 0));            \r
 157                     circle(cflow, Point(x, y) * mult, 2, CV_RGB(255, 0, 0));\r
 158                 }            \r
 159             }\r
 160 \r
 161         rectangle(cflow, (where.tl() + d) * mult, (where.br() + d - Point(1,1)) * mult, CV_RGB(0, 0, 255));    \r
 162         namedWindow(name, 1); imshow(name, cflow);\r
 163     }\r
 164 \r
 165     double angle = atan2((float)d.y, (float)d.x);\r
 166     double error = 0;\r
 167 \r
 168     bool all = true;\r
 169     Mat inner = flow(where);\r
 170     for(int y = 0; y < inner.rows; ++y)\r
 171         for(int x = 0; x < inner.cols; ++x)\r
 172         {\r
 173             const Point2f f = inner.at<Point2f>(y, x);\r
 174 \r
 175             if (f.x == 0 && f.y == 0)\r
 176                 continue;\r
 177 \r
 178             all = false;\r
 179 \r
 180             double a = atan2(f.y, f.x);\r
 181             error += fabs(angle - a);            \r
 182         }\r
 183         double res = all ? numeric_limits<double>::max() : error / (inner.cols * inner.rows);\r
 184 \r
 185         if (writeError)\r
 186             cout << "Error " + name << " = " << res << endl;\r
 187 \r
 188         return res;\r
 189 }\r
 190 \r
 191 \r
 192 Mat generateImage(const Size& sz, bool doBlur = true)\r
 193 {\r
 194     RNG rng;\r
 195     Mat mat(sz, CV_8U);\r
 196     mat = Scalar(0);\r
 197     for(int y = 0; y < mat.rows; ++y)\r
 198         for(int x = 0; x < mat.cols; ++x)\r
 199             mat.at<uchar>(y, x) = (uchar)rng;    \r
 200     if (doBlur)\r
 201         blur(mat, mat, Size(3, 3));\r
 202     return mat;\r
 203 }\r
 204 \r
 205 Mat generateSample(const Size& sz)\r
 206 {\r
 207     Mat smpl(sz, CV_8U);    \r
 208     smpl = Scalar(0);\r
 209     Point sc(smpl.cols/2, smpl.rows/2);\r
 210     rectangle(smpl, Point(0,0), sc - Point(1,1), Scalar(255), CV_FILLED);\r
 211     rectangle(smpl, sc, Point(smpl.cols, smpl.rows), Scalar(255), CV_FILLED);\r
 212     return smpl;\r
 213 }\r
 214 \r
 215 bool CV_OptFlowTest::runDense(const Point& d)\r
 216 {\r
 217     Size matSize(40, 40);\r
 218     Size movSize(8, 8);\r
 219         \r
 220     Mat smpl = generateSample(movSize);\r
 221     Mat prev = generateImage(matSize);    \r
 222     Mat curr = prev.clone();\r
 223 \r
 224     Rect rect(Point(prev.cols/2, prev.rows/2) - Point(movSize.width/2, movSize.height/2), movSize);\r
 225 \r
 226     Mat flowLK, flowBM, flowHS, flowFB, flowFB_G, flowBM_received, m1;\r
 227 \r
 228     m1 = prev(rect);                                smpl.copyTo(m1);\r
 229     m1 = curr(Rect(rect.tl() + d, rect.br() + d));  smpl.copyTo(m1);   \r
 230     \r
 231     calcOpticalFlowLK( prev, curr, Size(15, 15), flowLK);        \r
 232     calcOpticalFlowBM( prev, curr, Size(15, 15), Size(1, 1), Size(15, 15), 0, flowBM_received);       \r
 233     calcOpticalFlowHS( prev, curr, 0, 5, TermCriteria(TermCriteria::MAX_ITER, 400, 0), flowHS);                 \r
 234     calcOpticalFlowFarneback( prev, curr, flowFB, 0.5, 3, std::max(d.x, d.y) + 10, 100, 6, 2, 0);\r
 235     calcOpticalFlowFarneback( prev, curr, flowFB_G, 0.5, 3, std::max(d.x, d.y) + 10, 100, 6, 2, OPTFLOW_FARNEBACK_GAUSSIAN);            \r
 236 \r
 237     flowBM.create(prev.size(), CV_32FC2);\r
 238     flowBM = Scalar(0);    \r
 239     Point origin((flowBM.cols - flowBM_received.cols)/2, (flowBM.rows - flowBM_received.rows)/2);\r
 240     Mat wcp = flowBM(Rect(origin, flowBM_received.size()));\r
 241     flowBM_received.copyTo(wcp);\r
 242 \r
 243     double errorLK = showFlowAndCalcError("LK", prev, flowLK, rect, d);\r
 244     double errorBM = showFlowAndCalcError("BM", prev, flowBM, rect, d);\r
 245     double errorFB = showFlowAndCalcError("FB", prev, flowFB, rect, d);\r
 246     double errorFBG = showFlowAndCalcError("FBG", prev, flowFB_G, rect, d);\r
 247     double errorHS = showFlowAndCalcError("HS", prev, flowHS, rect, d); (void)errorHS;     \r
 248     //waitKey();   \r
 249 \r
 250     const double thres = 0.2;\r
 251     if (errorLK > thres || errorBM > thres || errorFB > thres || errorFBG > thres /*|| errorHS > thres /**/)\r
 252     {        \r
 253         ts->set_failed_test_info(CvTS::FAIL_MISMATCH);\r
 254         return false;\r
 255     }        \r
 256     return true;\r
 257 }\r
 258 \r
 259 \r
 260 bool CV_OptFlowTest::runSparse()\r
 261 {    \r
 262     Mat prev = imread(string(ts->get_data_path()) + "optflow/rock_1.bmp", 0);\r
 263     Mat next = imread(string(ts->get_data_path()) + "optflow/rock_2.bmp", 0);\r
 264 \r
 265     if (prev.empty() || next.empty())\r
 266     {\r
 267         ts->set_failed_test_info( CvTS::FAIL_INVALID_TEST_DATA );  \r
 268         return false;\r
 269     }\r
 270 \r
 271     Mat cprev, cnext;\r
 272     cvtColor(prev, cprev, CV_GRAY2BGR);\r
 273     cvtColor(next, cnext, CV_GRAY2BGR);\r
 274 \r
 275     vector<Point2f> prev_pts;\r
 276     vector<Point2f> next_ptsOpt;\r
 277     vector<Point2f> next_ptsAff;\r
 278     vector<uchar> status_Opt;\r
 279     vector<uchar> status_Aff;\r
 280     vector<float> error;\r
 281     vector<float> matrices;\r
 282 \r
 283     Size netSize(10, 10);\r
 284     Point2f center = Point(prev.cols/2, prev.rows/2);\r
 285 \r
 286     for(int i = 0 ; i < netSize.width; ++i)\r
 287         for(int j = 0 ; j < netSize.width; ++j)\r
 288         {\r
 289             Point2f p(i * float(prev.cols)/netSize.width, j * float(prev.rows)/netSize.height);\r
 290             prev_pts.push_back((p - center) * 0.5f + center);            \r
 291         }\r
 292 \r
 293     calcOpticalFlowPyrLK( prev, next, prev_pts, next_ptsOpt, status_Opt, error );\r
 294     calcAffineFlowPyrLK ( prev, next, prev_pts, next_ptsAff, status_Aff, error, matrices);\r
 295 \r
 296     const double expected_shift = 25;\r
 297     const double thres = 1;    \r
 298     for(size_t i = 0; i < prev_pts.size(); ++i)        \r
 299     {\r
 300         circle(cprev, prev_pts[i], 2, CV_RGB(255, 0, 0));               \r
 301 \r
 302         if (status_Opt[i])\r
 303         {            \r
 304             circle(cnext, next_ptsOpt[i], 2, CV_RGB(0, 0, 255));\r
 305             Point2f shift = prev_pts[i] - next_ptsOpt[i];\r
 306             \r
 307             double n = sqrt(shift.ddot(shift));\r
 308             if (fabs(n - expected_shift) > thres)\r
 309             {\r
 310                 ts->set_failed_test_info(CvTS::FAIL_MISMATCH);\r
 311                 return false;\r
 312             }\r
 313         }\r
 314 \r
 315         if (status_Aff[i])\r
 316         {            \r
 317             circle(cnext, next_ptsAff[i], 4, CV_RGB(0, 255, 0));\r
 318             Point2f shift = prev_pts[i] - next_ptsAff[i];\r
 319 \r
 320             double n = sqrt(shift.ddot(shift));\r
 321             if (fabs(n - expected_shift) > thres)\r
 322             {\r
 323                 ts->set_failed_test_info(CvTS::FAIL_MISMATCH);\r
 324                 return false;\r
 325             }\r
 326         }\r
 327         \r
 328     }\r
 329     \r
 330     /*namedWindow("P");  imshow("P", cprev);\r
 331     namedWindow("N"); imshow("N", cnext); \r
 332     waitKey();*/\r
 333     \r
 334     return true;\r
 335 }\r
 336 \r
 337 \r
 338 void CV_OptFlowTest::run( int /* start_from */)\r
 339 {       \r
 340 \r
 341     if (!runDense(Point(3, 0)))\r
 342         return;\r
 343 \r
 344     if (!runDense(Point(0, 3))) \r
 345         return;\r
 346 \r
 347     //if (!runDense(Point(3, 3))) return;  //probably LK works incorrectly in this case.\r
 348 \r
 349     if (!runSparse()) \r
 350         return;\r
 351 \r
 352     ts->set_failed_test_info(CvTS::OK);\r
 353 }\r
 354 \r
 355 CV_OptFlowTest optFlow_test;\r
 356 \r