← Back to branch summary

~paparazzi-uav/paparazzi/v5.0-manual

« back to all changes in this revision

Viewing changes to sw/ext/opencv_bebop/opencv/samples/tapi/tvl1_optical_flow.cpp

  • Committer: Paparazzi buildbot
  • Date: 2016-05-18 15:00:29 UTC
  • Revision ID: felix.ruess+docbot@gmail.com-20160518150029-e8lgzi5kvb4p7un9
Manual import commit 4b8bbb730080dac23cf816b98908dacfabe2a8ec from v5.0 branch.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sw/ext/opencv_bebop/opencv/samples/tapi/tvl1_optical_flow.cpp
 
1
#include <iostream>
 
2
#include <vector>
 
3
#include <iomanip>
 
4
 
 
5
#include "opencv2/core/ocl.hpp"
 
6
#include "opencv2/core/utility.hpp"
 
7
#include "opencv2/imgcodecs.hpp"
 
8
#include "opencv2/videoio.hpp"
 
9
#include "opencv2/highgui/highgui.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
 
 
20
static void workBegin()
 
21
{
 
22
    work_begin = getTickCount();
 
23
}
 
24
static void workEnd()
 
25
{
 
26
    work_end += (getTickCount() - work_begin);
 
27
}
 
28
static double getTime()
 
29
{
 
30
    return work_end * 1000. / getTickFrequency();
 
31
}
 
32
 
 
33
template <typename T> inline T clamp (T x, T a, T b)
 
34
{
 
35
    return ((x) > (a) ? ((x) < (b) ? (x) : (b)) : (a));
 
36
}
 
37
 
 
38
template <typename T> inline T mapValue(T x, T a, T b, T c, T d)
 
39
{
 
40
    x = clamp(x, a, b);
 
41
    return c + (d - c) * (x - a) / (b - a);
 
42
}
 
43
 
 
44
static void getFlowField(const Mat& u, const Mat& v, Mat& flowField)
 
45
{
 
46
    float maxDisplacement = 1.0f;
 
47
 
 
48
    for (int i = 0; i < u.rows; ++i)
 
49
    {
 
50
        const float* ptr_u = u.ptr<float>(i);
 
51
        const float* ptr_v = v.ptr<float>(i);
 
52
 
 
53
        for (int j = 0; j < u.cols; ++j)
 
54
        {
 
55
            float d = max(fabsf(ptr_u[j]), fabsf(ptr_v[j]));
 
56
 
 
57
            if (d > maxDisplacement)
 
58
                maxDisplacement = d;
 
59
        }
 
60
    }
 
61
 
 
62
    flowField.create(u.size(), CV_8UC4);
 
63
 
 
64
    for (int i = 0; i < flowField.rows; ++i)
 
65
    {
 
66
        const float* ptr_u = u.ptr<float>(i);
 
67
        const float* ptr_v = v.ptr<float>(i);
 
68
 
 
69
 
 
70
        Vec4b* row = flowField.ptr<Vec4b>(i);
 
71
 
 
72
        for (int j = 0; j < flowField.cols; ++j)
 
73
        {
 
74
            row[j][0] = 0;
 
75
            row[j][1] = static_cast<unsigned char> (mapValue (-ptr_v[j], -maxDisplacement, maxDisplacement, 0.0f, 255.0f));
 
76
            row[j][2] = static_cast<unsigned char> (mapValue ( ptr_u[j], -maxDisplacement, maxDisplacement, 0.0f, 255.0f));
 
77
            row[j][3] = 255;
 
78
        }
 
79
    }
 
80
}
 
81
 
 
82
 
 
83
int main(int argc, const char* argv[])
 
84
{
 
85
    const char* keys =
 
86
        "{ h help     | false           | print help message }"
 
87
        "{ l left     |                 | specify left image }"
 
88
        "{ r right    |                 | specify right image }"
 
89
        "{ o output   | tvl1_output.jpg | specify output save path }"
 
90
        "{ c camera   | 0               | enable camera capturing }"
 
91
        "{ m cpu_mode | false           | run without OpenCL }"
 
92
        "{ v video    |                 | use video as input }";
 
93
 
 
94
    CommandLineParser cmd(argc, argv, keys);
 
95
 
 
96
    if (cmd.has("help"))
 
97
    {
 
98
        cout << "Usage: pyrlk_optical_flow [options]" << endl;
 
99
        cout << "Available options:" << endl;
 
100
        cmd.printMessage();
 
101
        return EXIT_SUCCESS;
 
102
    }
 
103
 
 
104
    string fname0 = cmd.get<string>("l");
 
105
    string fname1 = cmd.get<string>("r");
 
106
    string vdofile = cmd.get<string>("v");
 
107
    string outpath = cmd.get<string>("o");
 
108
    bool useCPU = cmd.get<bool>("s");
 
109
    bool useCamera = cmd.get<bool>("c");
 
110
    int inputName = cmd.get<int>("c");
 
111
 
 
112
    UMat frame0, frame1;
 
113
    imread(fname0, cv::IMREAD_GRAYSCALE).copyTo(frame0);
 
114
    imread(fname1, cv::IMREAD_GRAYSCALE).copyTo(frame1);
 
115
    cv::Ptr<cv::DenseOpticalFlow> alg = cv::createOptFlow_DualTVL1();
 
116
 
 
117
    UMat flow;
 
118
    Mat show_flow;
 
119
    vector<UMat> flow_vec;
 
120
    if (frame0.empty() || frame1.empty())
 
121
        useCamera = true;
 
122
 
 
123
    if (useCamera)
 
124
    {
 
125
        VideoCapture capture;
 
126
        UMat frame, frameCopy;
 
127
        UMat frame0Gray, frame1Gray;
 
128
        UMat ptr0, ptr1;
 
129
 
 
130
        if(vdofile.empty())
 
131
            capture.open( inputName );
 
132
        else
 
133
            capture.open(vdofile.c_str());
 
134
 
 
135
        if(!capture.isOpened())
 
136
        {
 
137
            if(vdofile.empty())
 
138
                cout << "Capture from CAM " << inputName << " didn't work" << endl;
 
139
            else
 
140
                cout << "Capture from file " << vdofile << " failed" <<endl;
 
141
            goto nocamera;
 
142
        }
 
143
 
 
144
        cout << "In capture ..." << endl;
 
145
        for(int i = 0;; i++)
 
146
        {
 
147
            if( !capture.read(frame) )
 
148
                break;
 
149
 
 
150
            if (i == 0)
 
151
            {
 
152
                frame.copyTo( frame0 );
 
153
                cvtColor(frame0, frame0Gray, COLOR_BGR2GRAY);
 
154
            }
 
155
            else
 
156
            {
 
157
                if (i%2 == 1)
 
158
                {
 
159
                    frame.copyTo(frame1);
 
160
                    cvtColor(frame1, frame1Gray, COLOR_BGR2GRAY);
 
161
                    ptr0 = frame0Gray;
 
162
                    ptr1 = frame1Gray;
 
163
                }
 
164
                else
 
165
                {
 
166
                    frame.copyTo(frame0);
 
167
                    cvtColor(frame0, frame0Gray, COLOR_BGR2GRAY);
 
168
                    ptr0 = frame1Gray;
 
169
                    ptr1 = frame0Gray;
 
170
                }
 
171
 
 
172
                alg->calc(ptr0, ptr1, flow);
 
173
                split(flow, flow_vec);
 
174
 
 
175
                if (i%2 == 1)
 
176
                    frame1.copyTo(frameCopy);
 
177
                else
 
178
                    frame0.copyTo(frameCopy);
 
179
                getFlowField(flow_vec[0].getMat(ACCESS_READ), flow_vec[1].getMat(ACCESS_READ), show_flow);
 
180
                imshow("tvl1 optical flow field", show_flow);
 
181
            }
 
182
 
 
183
            char key = (char)waitKey(10);
 
184
            if (key == 27)
 
185
                break;
 
186
            else if (key == 'm' || key == 'M')
 
187
            {
 
188
                ocl::setUseOpenCL(!cv::ocl::useOpenCL());
 
189
                cout << "Switched to " << (ocl::useOpenCL() ? "OpenCL" : "CPU") << " mode\n";
 
190
            }
 
191
        }
 
192
 
 
193
        capture.release();
 
194
    }
 
195
    else
 
196
    {
 
197
nocamera:
 
198
        if (cmd.has("cpu_mode"))
 
199
        {
 
200
            ocl::setUseOpenCL(false);
 
201
            std::cout << "OpenCL was disabled" << std::endl;
 
202
        }
 
203
        for(int i = 0; i <= LOOP_NUM; i ++)
 
204
        {
 
205
            cout << "loop" << i << endl;
 
206
 
 
207
            if (i > 0) workBegin();
 
208
 
 
209
            alg->calc(frame0, frame1, flow);
 
210
            split(flow, flow_vec);
 
211
 
 
212
            if (i > 0 && i <= LOOP_NUM)
 
213
                workEnd();
 
214
 
 
215
            if (i == LOOP_NUM)
 
216
            {
 
217
                if (useCPU)
 
218
                    cout << "average CPU time (noCamera) : ";
 
219
                else
 
220
                    cout << "average GPU time (noCamera) : ";
 
221
                cout << getTime() / LOOP_NUM << " ms" << endl;
 
222
 
 
223
                getFlowField(flow_vec[0].getMat(ACCESS_READ), flow_vec[1].getMat(ACCESS_READ), show_flow);
 
224
                imshow("PyrLK [Sparse]", show_flow);
 
225
                imwrite(outpath, show_flow);
 
226
            }
 
227
        }
 
228
    }
 
229
 
 
230
    waitKey();
 
231
 
 
232
    return EXIT_SUCCESS;
 
233
}