pyrlk_optical_flow.cpp 8.14 KB
Newer Older
wester committed
1 2 3 4
#include <iostream>
#include <vector>
#include <iomanip>

wester committed
5 6
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/ocl/ocl.hpp"
wester committed
7 8 9 10
#include "opencv2/video/video.hpp"

using namespace std;
using namespace cv;
wester committed
11
using namespace cv::ocl;
wester committed
12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30

typedef unsigned char uchar;
#define LOOP_NUM 10
int64 work_begin = 0;
int64 work_end = 0;

static void workBegin()
{
    work_begin = getTickCount();
}
static void workEnd()
{
    work_end += (getTickCount() - work_begin);
}
static double getTime()
{
    return work_end * 1000. / getTickFrequency();
}

wester committed
31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47
static void download(const oclMat& d_mat, vector<Point2f>& vec)
{
    vec.clear();
    vec.resize(d_mat.cols);
    Mat mat(1, d_mat.cols, CV_32FC2, (void*)&vec[0]);
    d_mat.download(mat);
}

static void download(const oclMat& d_mat, vector<uchar>& vec)
{
    vec.clear();
    vec.resize(d_mat.cols);
    Mat mat(1, d_mat.cols, CV_8UC1, (void*)&vec[0]);
    d_mat.download(mat);
}

static void drawArrows(Mat& frame, const vector<Point2f>& prevPts, const vector<Point2f>& nextPts, const vector<uchar>& status,
wester committed
48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90
                       Scalar line_color = Scalar(0, 0, 255))
{
    for (size_t i = 0; i < prevPts.size(); ++i)
    {
        if (status[i])
        {
            int line_thickness = 1;

            Point p = prevPts[i];
            Point q = nextPts[i];

            double angle = atan2((double) p.y - q.y, (double) p.x - q.x);

            double hypotenuse = sqrt( (double)(p.y - q.y)*(p.y - q.y) + (double)(p.x - q.x)*(p.x - q.x) );

            if (hypotenuse < 1.0)
                continue;

            // Here we lengthen the arrow by a factor of three.
            q.x = (int) (p.x - 3 * hypotenuse * cos(angle));
            q.y = (int) (p.y - 3 * hypotenuse * sin(angle));

            // Now we draw the main line of the arrow.
            line(frame, p, q, line_color, line_thickness);

            // Now draw the tips of the arrow. I do some scaling so that the
            // tips look proportional to the main line of the arrow.

            p.x = (int) (q.x + 9 * cos(angle + CV_PI / 4));
            p.y = (int) (q.y + 9 * sin(angle + CV_PI / 4));
            line(frame, p, q, line_color, line_thickness);

            p.x = (int) (q.x + 9 * cos(angle - CV_PI / 4));
            p.y = (int) (q.y + 9 * sin(angle - CV_PI / 4));
            line(frame, p, q, line_color, line_thickness);
        }
    }
}


int main(int argc, const char* argv[])
{
    const char* keys =
wester committed
91 92 93 94 95 96 97 98 99
        "{ h   | help     | false           | print help message }"
        "{ l   | left     |                 | specify left image }"
        "{ r   | right    |                 | specify right image }"
        "{ c   | camera   | 0               | specify camera id }"
        "{ s   | use_cpu  | false           | use cpu or gpu to process the image }"
        "{ v   | video    |                 | use video as input }"
        "{ o   | output   | pyrlk_output.jpg| specify output save path when input is images }"
        "{ p   | points   | 1000            | specify points count [GoodFeatureToTrack] }"
        "{ m   | min_dist | 0               | specify minimal distance between points [GoodFeatureToTrack] }";
wester committed
100 101 102

    CommandLineParser cmd(argc, argv, keys);

wester committed
103
    if (cmd.get<bool>("help"))
wester committed
104 105 106
    {
        cout << "Usage: pyrlk_optical_flow [options]" << endl;
        cout << "Available options:" << endl;
wester committed
107
        cmd.printParams();
wester committed
108 109 110
        return EXIT_SUCCESS;
    }

wester committed
111 112 113 114 115 116 117 118
    bool defaultPicturesFail = false;
    string fname0 = cmd.get<string>("l");
    string fname1 = cmd.get<string>("r");
    string vdofile = cmd.get<string>("v");
    string outfile = cmd.get<string>("o");
    int points = cmd.get<int>("p");
    double minDist = cmd.get<double>("m");
    bool useCPU = cmd.get<bool>("s");
wester committed
119 120
    int inputName = cmd.get<int>("c");

wester committed
121 122 123 124 125
    oclMat d_nextPts, d_status;
    GoodFeaturesToTrackDetector_OCL d_features(points);
    Mat frame0 = imread(fname0, cv::IMREAD_GRAYSCALE);
    Mat frame1 = imread(fname1, cv::IMREAD_GRAYSCALE);
    PyrLKOpticalFlow d_pyrLK;
wester committed
126 127 128 129 130 131 132 133 134
    vector<cv::Point2f> pts(points);
    vector<cv::Point2f> nextPts(points);
    vector<unsigned char> status(points);
    vector<float> err;

    cout << "Points count : " << points << endl << endl;

    if (frame0.empty() || frame1.empty())
    {
wester committed
135 136 137 138
        CvCapture* capture = 0;
        Mat frame, frameCopy;
        Mat frame0Gray, frame1Gray;
        Mat ptr0, ptr1;
wester committed
139 140

        if(vdofile.empty())
wester committed
141
            capture = cvCaptureFromCAM( inputName );
wester committed
142
        else
wester committed
143
            capture = cvCreateFileCapture(vdofile.c_str());
wester committed
144 145

        int c = inputName ;
wester committed
146
        if(!capture)
wester committed
147 148 149 150 151 152 153 154 155 156 157 158 159
        {
            if(vdofile.empty())
                cout << "Capture from CAM " << c << " didn't work" << endl;
            else
                cout << "Capture from file " << vdofile << " failed" <<endl;
            if (defaultPicturesFail)
                return EXIT_FAILURE;
            goto nocamera;
        }

        cout << "In capture ..." << endl;
        for(int i = 0;; i++)
        {
wester committed
160 161
            frame = cvQueryFrame( capture );
            if( frame.empty() )
wester committed
162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185
                break;

            if (i == 0)
            {
                frame.copyTo( frame0 );
                cvtColor(frame0, frame0Gray, COLOR_BGR2GRAY);
            }
            else
            {
                if (i%2 == 1)
                {
                    frame.copyTo(frame1);
                    cvtColor(frame1, frame1Gray, COLOR_BGR2GRAY);
                    ptr0 = frame0Gray;
                    ptr1 = frame1Gray;
                }
                else
                {
                    frame.copyTo(frame0);
                    cvtColor(frame0, frame0Gray, COLOR_BGR2GRAY);
                    ptr0 = frame1Gray;
                    ptr1 = frame0Gray;
                }

wester committed
186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204
                if (useCPU)
                {
                    pts.clear();
                    goodFeaturesToTrack(ptr0, pts, points, 0.01, 0.0);
                    if(pts.size() == 0)
                        continue;
                    calcOpticalFlowPyrLK(ptr0, ptr1, pts, nextPts, status, err);
                }
                else
                {
                    oclMat d_img(ptr0), d_prevPts;
                    d_features(d_img, d_prevPts);
                    if(!d_prevPts.rows || !d_prevPts.cols)
                        continue;
                    d_pyrLK.sparse(d_img, oclMat(ptr1), d_prevPts, d_nextPts, d_status);
                    d_features.downloadPoints(d_prevPts,pts);
                    download(d_nextPts, nextPts);
                    download(d_status, status);
                }
wester committed
205 206 207 208 209 210 211 212
                if (i%2 == 1)
                    frame1.copyTo(frameCopy);
                else
                    frame0.copyTo(frameCopy);
                drawArrows(frameCopy, pts, nextPts, status, Scalar(255, 0, 0));
                imshow("PyrLK [Sparse]", frameCopy);
            }

wester committed
213
            if( waitKey( 10 ) >= 0 )
wester committed
214 215
                break;
        }
wester committed
216 217

        cvReleaseCapture( &capture );
wester committed
218 219 220 221 222 223 224 225 226
    }
    else
    {
nocamera:
        for(int i = 0; i <= LOOP_NUM; i ++)
        {
            cout << "loop" << i << endl;
            if (i > 0) workBegin();

wester committed
227 228 229 230 231 232 233 234 235 236 237 238 239 240
            if (useCPU)
            {
                goodFeaturesToTrack(frame0, pts, points, 0.01, minDist);
                calcOpticalFlowPyrLK(frame0, frame1, pts, nextPts, status, err);
            }
            else
            {
                oclMat d_img(frame0), d_prevPts;
                d_features(d_img, d_prevPts);
                d_pyrLK.sparse(d_img, oclMat(frame1), d_prevPts, d_nextPts, d_status);
                d_features.downloadPoints(d_prevPts, pts);
                download(d_nextPts, nextPts);
                download(d_status, status);
            }
wester committed
241 242 243 244 245 246

            if (i > 0 && i <= LOOP_NUM)
                workEnd();

            if (i == LOOP_NUM)
            {
wester committed
247 248 249 250
                if (useCPU)
                    cout << "average CPU time (noCamera) : ";
                else
                    cout << "average GPU time (noCamera) : ";
wester committed
251 252 253 254 255 256 257 258 259 260 261 262 263 264

                cout << getTime() / LOOP_NUM << " ms" << endl;

                drawArrows(frame0, pts, nextPts, status, Scalar(255, 0, 0));
                imshow("PyrLK [Sparse]", frame0);
                imwrite(outfile, frame0);
            }
        }
    }

    waitKey();

    return EXIT_SUCCESS;
}