1#include <iostream>
2#include <vector>
3#include <iomanip>
4
5#include "opencv2/core/utility.hpp"
6#include "opencv2/imgcodecs.hpp"
7#include "opencv2/videoio.hpp"
8#include "opencv2/highgui.hpp"
9#include "opencv2/core/ocl.hpp"
10#include "opencv2/video/video.hpp"
11
12using namespace std;
13using namespace cv;
14
15typedef unsigned char uchar;
16#define LOOP_NUM 10
17int64 work_begin = 0;
18int64 work_end = 0;
19
20static void workBegin()
21{
22    work_begin = getTickCount();
23}
24static void workEnd()
25{
26    work_end += (getTickCount() - work_begin);
27}
28static double getTime()
29{
30    return work_end * 1000. / getTickFrequency();
31}
32
33static void drawArrows(UMat& _frame, const vector<Point2f>& prevPts, const vector<Point2f>& nextPts, const vector<uchar>& status,
34                       Scalar line_color = Scalar(0, 0, 255))
35{
36    Mat frame = _frame.getMat(ACCESS_WRITE);
37    for (size_t i = 0; i < prevPts.size(); ++i)
38    {
39        if (status[i])
40        {
41            int line_thickness = 1;
42
43            Point p = prevPts[i];
44            Point q = nextPts[i];
45
46            double angle = atan2((double) p.y - q.y, (double) p.x - q.x);
47
48            double hypotenuse = sqrt( (double)(p.y - q.y)*(p.y - q.y) + (double)(p.x - q.x)*(p.x - q.x) );
49
50            if (hypotenuse < 1.0)
51                continue;
52
53            // Here we lengthen the arrow by a factor of three.
54            q.x = (int) (p.x - 3 * hypotenuse * cos(angle));
55            q.y = (int) (p.y - 3 * hypotenuse * sin(angle));
56
57            // Now we draw the main line of the arrow.
58            line(frame, p, q, line_color, line_thickness);
59
60            // Now draw the tips of the arrow. I do some scaling so that the
61            // tips look proportional to the main line of the arrow.
62
63            p.x = (int) (q.x + 9 * cos(angle + CV_PI / 4));
64            p.y = (int) (q.y + 9 * sin(angle + CV_PI / 4));
65            line(frame, p, q, line_color, line_thickness);
66
67            p.x = (int) (q.x + 9 * cos(angle - CV_PI / 4));
68            p.y = (int) (q.y + 9 * sin(angle - CV_PI / 4));
69            line(frame, p, q, line_color, line_thickness);
70        }
71    }
72}
73
74
75int main(int argc, const char* argv[])
76{
77    const char* keys =
78        "{ h help           | false           | print help message }"
79        "{ l left           |                 | specify left image }"
80        "{ r right          |                 | specify right image }"
81        "{ c camera         | 0               | enable camera capturing }"
82        "{ v video          |                 | use video as input }"
83        "{ o output         | pyrlk_output.jpg| specify output save path when input is images }"
84        "{ points           | 1000            | specify points count [GoodFeatureToTrack] }"
85        "{ min_dist         | 0               | specify minimal distance between points [GoodFeatureToTrack] }"
86        "{ m cpu_mode       | false           | run without OpenCL }";
87
88    CommandLineParser cmd(argc, argv, keys);
89
90    if (cmd.has("help"))
91    {
92        cout << "Usage: pyrlk_optical_flow [options]" << endl;
93        cout << "Available options:" << endl;
94        cmd.printMessage();
95        return EXIT_SUCCESS;
96    }
97
98    bool defaultPicturesFail = true;
99    string fname0 = cmd.get<string>("left");
100    string fname1 = cmd.get<string>("right");
101    string vdofile = cmd.get<string>("video");
102    string outfile = cmd.get<string>("output");
103    int points = cmd.get<int>("points");
104    double minDist = cmd.get<double>("min_dist");
105    int inputName = cmd.get<int>("c");
106
107    UMat frame0;
108    imread(fname0, cv::IMREAD_GRAYSCALE).copyTo(frame0);
109    UMat frame1;
110    imread(fname1, cv::IMREAD_GRAYSCALE).copyTo(frame1);
111
112    vector<cv::Point2f> pts(points);
113    vector<cv::Point2f> nextPts(points);
114    vector<unsigned char> status(points);
115    vector<float> err;
116
117    cout << "Points count : " << points << endl << endl;
118
119    if (frame0.empty() || frame1.empty())
120    {
121        VideoCapture capture;
122        UMat frame, frameCopy;
123        UMat frame0Gray, frame1Gray;
124        UMat ptr0, ptr1;
125
126        if(vdofile.empty())
127            capture.open( inputName );
128        else
129            capture.open(vdofile.c_str());
130
131        int c = inputName ;
132        if(!capture.isOpened())
133        {
134            if(vdofile.empty())
135                cout << "Capture from CAM " << c << " didn't work" << endl;
136            else
137                cout << "Capture from file " << vdofile << " failed" <<endl;
138            if (defaultPicturesFail)
139                return EXIT_FAILURE;
140            goto nocamera;
141        }
142
143        cout << "In capture ..." << endl;
144        for(int i = 0;; i++)
145        {
146            if( !capture.read(frame) )
147                break;
148
149            if (i == 0)
150            {
151                frame.copyTo( frame0 );
152                cvtColor(frame0, frame0Gray, COLOR_BGR2GRAY);
153            }
154            else
155            {
156                if (i%2 == 1)
157                {
158                    frame.copyTo(frame1);
159                    cvtColor(frame1, frame1Gray, COLOR_BGR2GRAY);
160                    ptr0 = frame0Gray;
161                    ptr1 = frame1Gray;
162                }
163                else
164                {
165                    frame.copyTo(frame0);
166                    cvtColor(frame0, frame0Gray, COLOR_BGR2GRAY);
167                    ptr0 = frame1Gray;
168                    ptr1 = frame0Gray;
169                }
170
171
172                pts.clear();
173                goodFeaturesToTrack(ptr0, pts, points, 0.01, 0.0);
174                if(pts.size() == 0)
175                    continue;
176                calcOpticalFlowPyrLK(ptr0, ptr1, pts, nextPts, status, err);
177
178                if (i%2 == 1)
179                    frame1.copyTo(frameCopy);
180                else
181                    frame0.copyTo(frameCopy);
182                drawArrows(frameCopy, pts, nextPts, status, Scalar(255, 0, 0));
183                imshow("PyrLK [Sparse]", frameCopy);
184            }
185            char key = (char)waitKey(10);
186
187            if (key == 27)
188                break;
189            else if (key == 'm' || key == 'M')
190            {
191                ocl::setUseOpenCL(!cv::ocl::useOpenCL());
192                cout << "Switched to " << (ocl::useOpenCL() ? "OpenCL" : "CPU") << " mode\n";
193            }
194        }
195        capture.release();
196    }
197    else
198    {
199nocamera:
200        if (cmd.has("cpu_mode"))
201        {
202            ocl::setUseOpenCL(false);
203            std::cout << "OpenCL was disabled" << std::endl;
204        }
205        for(int i = 0; i <= LOOP_NUM; i ++)
206        {
207            cout << "loop" << i << endl;
208            if (i > 0) workBegin();
209
210            goodFeaturesToTrack(frame0, pts, points, 0.01, minDist);
211            calcOpticalFlowPyrLK(frame0, frame1, pts, nextPts, status, err);
212
213            if (i > 0 && i <= LOOP_NUM)
214                workEnd();
215
216            if (i == LOOP_NUM)
217            {
218                cout << "average time (noCamera) : ";
219
220                cout << getTime() / LOOP_NUM << " ms" << endl;
221
222                drawArrows(frame0, pts, nextPts, status, Scalar(255, 0, 0));
223                imshow("PyrLK [Sparse]", frame0);
224                imwrite(outfile, frame0);
225            }
226        }
227    }
228
229    waitKey();
230
231    return EXIT_SUCCESS;
232}
233