samples/ocl/pyrlk_optical_flow.cpp

   1 #include <iostream>
   2 #include <vector>
   3 #include <iomanip>
   4
   5 #include "opencv2/highgui/highgui.hpp"
   6 #include "opencv2/ocl/ocl.hpp"
   7 #include "opencv2/video/video.hpp"
   8
   9 using namespace std;
  10 using namespace cv;
  11 using namespace cv::ocl;
  12
  13 typedef unsigned char uchar;
  14 #define LOOP_NUM 10
  15 int64 work_begin = 0;
  16 int64 work_end = 0;
  17
  18 static void workBegin()
  19 {
  20     work_begin = getTickCount();
  21 }
  22 static void workEnd()
  23 {
  24     work_end += (getTickCount() - work_begin);
  25 }
  26 static double getTime()
  27 {
  28     return work_end * 1000. / getTickFrequency();
  29 }
  30
  31 static void download(const oclMat& d_mat, vector<Point2f>& vec)
  32 {
  33     vec.clear();
  34     vec.resize(d_mat.cols);
  35     Mat mat(1, d_mat.cols, CV_32FC2, (void*)&vec[0]);
  36     d_mat.download(mat);
  37 }
  38
  39 static void download(const oclMat& d_mat, vector<uchar>& vec)
  40 {
  41     vec.clear();
  42     vec.resize(d_mat.cols);
  43     Mat mat(1, d_mat.cols, CV_8UC1, (void*)&vec[0]);
  44     d_mat.download(mat);
  45 }
  46
  47 static void drawArrows(Mat& frame, const vector<Point2f>& prevPts, const vector<Point2f>& nextPts, const vector<uchar>& status, Scalar line_color = Scalar(0, 0, 255))
  48 {
  49     for (size_t i = 0; i < prevPts.size(); ++i)
  50     {
  51         if (status[i])
  52         {
  53             int line_thickness = 1;
  54
  55             Point p = prevPts[i];
  56             Point q = nextPts[i];
  57
  58             double angle = atan2((double) p.y - q.y, (double) p.x - q.x);
  59
  60             double hypotenuse = sqrt( (double)(p.y - q.y)*(p.y - q.y) + (double)(p.x - q.x)*(p.x - q.x) );
  61
  62             if (hypotenuse < 1.0)
  63                 continue;
  64
  65             // Here we lengthen the arrow by a factor of three.
  66             q.x = (int) (p.x - 3 * hypotenuse * cos(angle));
  67             q.y = (int) (p.y - 3 * hypotenuse * sin(angle));
  68
  69             // Now we draw the main line of the arrow.
  70             line(frame, p, q, line_color, line_thickness);
  71
  72             // Now draw the tips of the arrow. I do some scaling so that the
  73             // tips look proportional to the main line of the arrow.
  74
  75             p.x = (int) (q.x + 9 * cos(angle + CV_PI / 4));
  76             p.y = (int) (q.y + 9 * sin(angle + CV_PI / 4));
  77             line(frame, p, q, line_color, line_thickness);
  78
  79             p.x = (int) (q.x + 9 * cos(angle - CV_PI / 4));
  80             p.y = (int) (q.y + 9 * sin(angle - CV_PI / 4));
  81             line(frame, p, q, line_color, line_thickness);
  82         }
  83     }
  84 }
  85
  86
  87 int main(int argc, const char* argv[])
  88 {
  89     static std::vector<Info> ocl_info;
  90     ocl::getDevice(ocl_info);
  91     //if you want to use undefault device, set it here
  92     setDevice(ocl_info[0]);
  93
  94     //set this to save kernel compile time from second time you run
  95     ocl::setBinpath("./");
  96     const char* keys =
  97         "{ h   | help     | false           | print help message }"
  98         "{ l   | left     |                 | specify left image }"
  99         "{ r   | right    |                 | specify right image }"
 100         "{ c   | camera   | 0               | specify camera id }"
 101         "{ s   | use_cpu  | false           | use cpu or gpu to process the image }"
 102         "{ v   | video    |                 | use video as input }"
 103         "{ o   | output   | pyrlk_output.jpg| specify output save path when input is images }"
 104         "{ p   | points   | 1000            | specify points count [GoodFeatureToTrack] }"
 105         "{ m   | min_dist | 0               | specify minimal distance between points [GoodFeatureToTrack] }";
 106
 107     CommandLineParser cmd(argc, argv, keys);
 108
 109     if (cmd.get<bool>("help"))
 110     {
 111         cout << "Usage: pyrlk_optical_flow [options]" << endl;
 112         cout << "Avaible options:" << endl;
 113         cmd.printParams();
 114         return 0;
 115     }
 116
 117     bool defaultPicturesFail = false;
 118     string fname0 = cmd.get<string>("l");
 119     string fname1 = cmd.get<string>("r");
 120     string vdofile = cmd.get<string>("v");
 121     string outfile = cmd.get<string>("o");
 122     int points = cmd.get<int>("p");
 123     double minDist = cmd.get<double>("m");
 124     bool useCPU = cmd.get<bool>("s");
 125     int inputName = cmd.get<int>("c");
 126
 127     oclMat d_nextPts, d_status;
 128     GoodFeaturesToTrackDetector_OCL d_features(points);
 129     Mat frame0 = imread(fname0, cv::IMREAD_GRAYSCALE);
 130     Mat frame1 = imread(fname1, cv::IMREAD_GRAYSCALE);
 131     PyrLKOpticalFlow d_pyrLK;
 132     vector<cv::Point2f> pts(points);
 133     vector<cv::Point2f> nextPts(points);
 134     vector<unsigned char> status(points);
 135     vector<float> err;
 136
 137     cout << "Points count : " << points << endl << endl;
 138
 139     if (frame0.empty() || frame1.empty())
 140     {
 141         CvCapture* capture = 0;
 142         Mat frame, frameCopy;
 143         Mat frame0Gray, frame1Gray;
 144         Mat ptr0, ptr1;
 145
 146         if(vdofile == "")
 147             capture = cvCaptureFromCAM( inputName );
 148         else
 149             capture = cvCreateFileCapture(vdofile.c_str());
 150
 151         int c = inputName ;
 152         if(!capture)
 153         {
 154             if(vdofile == "")
 155                 cout << "Capture from CAM " << c << " didn't work" << endl;
 156             else
 157                 cout << "Capture from file " << vdofile << " failed" <<endl;
 158             if (defaultPicturesFail)
 159             {
 160                 return -1;
 161             }
 162             goto nocamera;
 163         }
 164
 165         cout << "In capture ..." << endl;
 166         for(int i = 0;; i++)
 167         {
 168             frame = cvQueryFrame( capture );
 169             if( frame.empty() )
 170                 break;
 171
 172             if (i == 0)
 173             {
 174                 frame.copyTo( frame0 );
 175                 cvtColor(frame0, frame0Gray, COLOR_BGR2GRAY);
 176             }
 177             else
 178             {
 179                 if (i%2 == 1)
 180                 {
 181                     frame.copyTo(frame1);
 182                     cvtColor(frame1, frame1Gray, COLOR_BGR2GRAY);
 183                     ptr0 = frame0Gray;
 184                     ptr1 = frame1Gray;
 185                 }
 186                 else
 187                 {
 188                     frame.copyTo(frame0);
 189                     cvtColor(frame0, frame0Gray, COLOR_BGR2GRAY);
 190                     ptr0 = frame1Gray;
 191                     ptr1 = frame0Gray;
 192                 }
 193
 194                 if (useCPU)
 195                 {
 196                     pts.clear();
 197                     goodFeaturesToTrack(ptr0, pts, points, 0.01, 0.0);
 198                     if(pts.size() == 0)
 199                         continue;
 200                     calcOpticalFlowPyrLK(ptr0, ptr1, pts, nextPts, status, err);
 201                 }
 202                 else
 203                 {
 204                     oclMat d_img(ptr0), d_prevPts;
 205                     d_features(d_img, d_prevPts);
 206                     if(!d_prevPts.rows || !d_prevPts.cols)
 207                         continue;
 208                     d_pyrLK.sparse(d_img, oclMat(ptr1), d_prevPts, d_nextPts, d_status);
 209                     d_features.downloadPoints(d_prevPts,pts);
 210                     download(d_nextPts, nextPts);
 211                     download(d_status, status);
 212                 }
 213                 if (i%2 == 1)
 214                     frame1.copyTo(frameCopy);
 215                 else
 216                     frame0.copyTo(frameCopy);
 217                 drawArrows(frameCopy, pts, nextPts, status, Scalar(255, 0, 0));
 218                 imshow("PyrLK [Sparse]", frameCopy);
 219             }
 220
 221             if( waitKey( 10 ) >= 0 )
 222                 goto _cleanup_;
 223         }
 224
 225         waitKey(0);
 226
 227 _cleanup_:
 228         cvReleaseCapture( &capture );
 229     }
 230     else
 231     {
 232 nocamera:
 233         for(int i = 0; i <= LOOP_NUM; i ++)
 234         {
 235             cout << "loop" << i << endl;
 236             if (i > 0) workBegin();
 237
 238             if (useCPU)
 239             {
 240                 goodFeaturesToTrack(frame0, pts, points, 0.01, minDist);
 241                 calcOpticalFlowPyrLK(frame0, frame1, pts, nextPts, status, err);
 242             }
 243             else
 244             {
 245                 oclMat d_img(frame0), d_prevPts;
 246                 d_features(d_img, d_prevPts);
 247                 d_pyrLK.sparse(d_img, oclMat(frame1), d_prevPts, d_nextPts, d_status);
 248                 d_features.downloadPoints(d_prevPts, pts);
 249                 download(d_nextPts, nextPts);
 250                 download(d_status, status);
 251             }
 252
 253             if (i > 0 && i <= LOOP_NUM)
 254                 workEnd();
 255
 256             if (i == LOOP_NUM)
 257             {
 258                 if (useCPU)
 259                     cout << "average CPU time (noCamera) : ";
 260                 else
 261                     cout << "average GPU time (noCamera) : ";
 262
 263                 cout << getTime() / LOOP_NUM << " ms" << endl;
 264
 265                 drawArrows(frame0, pts, nextPts, status, Scalar(255, 0, 0));
 266                 imshow("PyrLK [Sparse]", frame0);
 267                 imwrite(outfile, frame0);
 268             }
 269         }
 270     }
 271
 272     waitKey();
 273
 274     return 0;
 275 }