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
12 using namespace std;
13 using namespace cv;
14
15 typedef unsigned char uchar;
16 #define LOOP_NUM 10
17 int64 work_begin = 0;
18 int64 work_end = 0;
19
workBegin()20 static void workBegin()
21 {
22 work_begin = getTickCount();
23 }
workEnd()24 static void workEnd()
25 {
26 work_end += (getTickCount() - work_begin);
27 }
getTime()28 static double getTime()
29 {
30 return work_end * 1000. / getTickFrequency();
31 }
32
drawArrows(UMat & _frame,const vector<Point2f> & prevPts,const vector<Point2f> & nextPts,const vector<uchar> & status,Scalar line_color=Scalar (0,0,255))33 static 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
main(int argc,const char * argv[])75 int main(int argc, const char* argv[])
76 {
77 const char* keys =
78 "{ h help | | 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 = samples::findFile(cmd.get<string>("left"));
100 string fname1 = samples::findFile(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, IMREAD_GRAYSCALE).copyTo(frame0);
109 UMat frame1;
110 imread(fname1, 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 {
199 nocamera:
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