#include "cvtest.h"#include <string>#include <iostream>#include <fstream>#include <iterator>#include <limits>#include "cvaux.h"using namespace cv;using namespace std;class CV_OptFlowTest : public CvTest{public: CV_OptFlowTest(); ~CV_OptFlowTest(); protected: void run(int); bool runDense(const Point& shift = Point(3, 0)); bool runSparse();};CV_OptFlowTest::CV_OptFlowTest(): CvTest( "optflow-all", "?" ){ support_testing_modes = CvTS::CORRECTNESS_CHECK_MODE;}CV_OptFlowTest::~CV_OptFlowTest() {}Mat copnvert2flow(const Mat& velx, const Mat& vely){ Mat flow(velx.size(), CV_32FC2); for(int y = 0 ; y < flow.rows; ++y) for(int x = 0 ; x < flow.cols; ++x) flow.at<Point2f>(y, x) = Point2f(velx.at<float>(y, x), vely.at<float>(y, x)); return flow;}void calcOpticalFlowLK( const Mat& prev, const Mat& curr, Size winSize, Mat& flow ){ Mat velx(prev.size(), CV_32F), vely(prev.size(), CV_32F); CvMat cvvelx = velx; CvMat cvvely = vely; CvMat cvprev = prev; CvMat cvcurr = curr; cvCalcOpticalFlowLK( &cvprev, &cvcurr, winSize, &cvvelx, &cvvely ); flow = copnvert2flow(velx, vely);}void calcOpticalFlowBM( const Mat& prev, const Mat& curr, Size bSize, Size shiftSize, Size maxRange, int usePrevious, Mat& flow ){ Size sz((curr.cols - bSize.width)/shiftSize.width, (curr.rows - bSize.height)/shiftSize.height); Mat velx(sz, CV_32F), vely(sz, CV_32F); CvMat cvvelx = velx; CvMat cvvely = vely; CvMat cvprev = prev; CvMat cvcurr = curr; cvCalcOpticalFlowBM( &cvprev, &cvcurr, bSize, shiftSize, maxRange, usePrevious, &cvvelx, &cvvely); flow = copnvert2flow(velx, vely);}void calcOpticalFlowHS( const Mat& prev, const Mat& curr, int usePrevious, double lambda, TermCriteria criteria, Mat& flow){ Mat velx(prev.size(), CV_32F), vely(prev.size(), CV_32F); CvMat cvvelx = velx; CvMat cvvely = vely; CvMat cvprev = prev; CvMat cvcurr = curr; cvCalcOpticalFlowHS( &cvprev, &cvcurr, usePrevious, &cvvelx, &cvvely, lambda, criteria ); flow = copnvert2flow(velx, vely);}void calcAffineFlowPyrLK( const Mat& prev, const Mat& curr, const vector<Point2f>& prev_features, vector<Point2f>& curr_features, vector<uchar>& status, vector<float>& track_error, vector<float>& matrices, TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,30, 0.01), Size win_size = Size(15, 15), int level = 3, int flags = 0){ CvMat cvprev = prev; CvMat cvcurr = curr; size_t count = prev_features.size(); curr_features.resize(count); status.resize(count); track_error.resize(count); matrices.resize(count * 6); cvCalcAffineFlowPyrLK( &cvprev, &cvcurr, 0, 0, (const CvPoint2D32f*)&prev_features[0], (CvPoint2D32f*)&curr_features[0], &matrices[0], (int)count, win_size, level, (char*)&status[0], &track_error[0], criteria, flags );}double showFlowAndCalcError(const string& name, const Mat& gray, const Mat& flow, const Rect& where, const Point& d, bool showImages = false, bool writeError = false){ const int mult = 16; if (showImages) { Mat tmp, cflow; resize(gray, tmp, gray.size() * mult, 0, 0, INTER_NEAREST); cvtColor(tmp, cflow, CV_GRAY2BGR); const float m2 = 0.3f; const float minVel = 0.1f; for(int y = 0; y < flow.rows; ++y) for(int x = 0; x < flow.cols; ++x) { Point2f f = flow.at<Point2f>(y, x); if (f.x * f.x + f.y * f.y > minVel * minVel) { Point p1 = Point(x, y) * mult; Point p2 = Point(cvRound((x + f.x*m2) * mult), cvRound((y + f.y*m2) * mult)); line(cflow, p1, p2, CV_RGB(0, 255, 0)); circle(cflow, Point(x, y) * mult, 2, CV_RGB(255, 0, 0)); } } rectangle(cflow, (where.tl() + d) * mult, (where.br() + d - Point(1,1)) * mult, CV_RGB(0, 0, 255)); namedWindow(name, 1); imshow(name, cflow); } double angle = atan2((float)d.y, (float)d.x); double error = 0; bool all = true; Mat inner = flow(where); for(int y = 0; y < inner.rows; ++y) for(int x = 0; x < inner.cols; ++x) { const Point2f f = inner.at<Point2f>(y, x); if (f.x == 0 && f.y == 0) continue; all = false; double a = atan2(f.y, f.x); error += fabs(angle - a); } double res = all ? numeric_limits<double>::max() : error / (inner.cols * inner.rows); if (writeError) cout << "Error " + name << " = " << res << endl; return res;}Mat generateImage(const Size& sz, bool doBlur = true){ RNG rng; Mat mat(sz, CV_8U); mat = Scalar(0); for(int y = 0; y < mat.rows; ++y) for(int x = 0; x < mat.cols; ++x) mat.at<uchar>(y, x) = (uchar)rng; if (doBlur) blur(mat, mat, Size(3, 3)); return mat;}Mat generateSample(const Size& sz){ Mat smpl(sz, CV_8U); smpl = Scalar(0); Point sc(smpl.cols/2, smpl.rows/2); rectangle(smpl, Point(0,0), sc - Point(1,1), Scalar(255), CV_FILLED); rectangle(smpl, sc, Point(smpl.cols, smpl.rows), Scalar(255), CV_FILLED); return smpl;}bool CV_OptFlowTest::runDense(const Point& d){ Size matSize(40, 40); Size movSize(8, 8); Mat smpl = generateSample(movSize); Mat prev = generateImage(matSize); Mat curr = prev.clone(); Rect rect(Point(prev.cols/2, prev.rows/2) - Point(movSize.width/2, movSize.height/2), movSize); Mat flowLK, flowBM, flowHS, flowFB, flowFB_G, flowBM_received, m1; m1 = prev(rect); smpl.copyTo(m1); m1 = curr(Rect(rect.tl() + d, rect.br() + d)); smpl.copyTo(m1); calcOpticalFlowLK( prev, curr, Size(15, 15), flowLK); calcOpticalFlowBM( prev, curr, Size(15, 15), Size(1, 1), Size(15, 15), 0, flowBM_received); calcOpticalFlowHS( prev, curr, 0, 5, TermCriteria(TermCriteria::MAX_ITER, 400, 0), flowHS); calcOpticalFlowFarneback( prev, curr, flowFB, 0.5, 3, std::max(d.x, d.y) + 10, 100, 6, 2, 0); calcOpticalFlowFarneback( prev, curr, flowFB_G, 0.5, 3, std::max(d.x, d.y) + 10, 100, 6, 2, OPTFLOW_FARNEBACK_GAUSSIAN); flowBM.create(prev.size(), CV_32FC2); flowBM = Scalar(0); Point origin((flowBM.cols - flowBM_received.cols)/2, (flowBM.rows - flowBM_received.rows)/2); Mat wcp = flowBM(Rect(origin, flowBM_received.size())); flowBM_received.copyTo(wcp); double errorLK = showFlowAndCalcError("LK", prev, flowLK, rect, d); double errorBM = showFlowAndCalcError("BM", prev, flowBM, rect, d); double errorFB = showFlowAndCalcError("FB", prev, flowFB, rect, d); double errorFBG = showFlowAndCalcError("FBG", prev, flowFB_G, rect, d); double errorHS = showFlowAndCalcError("HS", prev, flowHS, rect, d); (void)errorHS; //waitKey(); const double thres = 0.2; if (errorLK > thres || errorBM > thres || errorFB > thres || errorFBG > thres /*|| errorHS > thres */) { ts->set_failed_test_info(CvTS::FAIL_MISMATCH); return false; } return true;}bool CV_OptFlowTest::runSparse(){ Mat prev = imread(string(ts->get_data_path()) + "optflow/rock_1.bmp", 0); Mat next = imread(string(ts->get_data_path()) + "optflow/rock_2.bmp", 0); if (prev.empty() || next.empty()) { ts->set_failed_test_info( CvTS::FAIL_INVALID_TEST_DATA ); return false; } Mat cprev, cnext; cvtColor(prev, cprev, CV_GRAY2BGR); cvtColor(next, cnext, CV_GRAY2BGR); vector<Point2f> prev_pts; vector<Point2f> next_ptsOpt; vector<Point2f> next_ptsAff; vector<uchar> status_Opt; vector<uchar> status_Aff; vector<float> error; vector<float> matrices; Size netSize(10, 10); Point2f center = Point(prev.cols/2, prev.rows/2); for(int i = 0 ; i < netSize.width; ++i) for(int j = 0 ; j < netSize.width; ++j) { Point2f p(i * float(prev.cols)/netSize.width, j * float(prev.rows)/netSize.height); prev_pts.push_back((p - center) * 0.5f + center); } calcOpticalFlowPyrLK( prev, next, prev_pts, next_ptsOpt, status_Opt, error ); calcAffineFlowPyrLK ( prev, next, prev_pts, next_ptsAff, status_Aff, error, matrices); const double expected_shift = 25; const double thres = 1; for(size_t i = 0; i < prev_pts.size(); ++i) { circle(cprev, prev_pts[i], 2, CV_RGB(255, 0, 0)); if (status_Opt[i]) { circle(cnext, next_ptsOpt[i], 2, CV_RGB(0, 0, 255)); Point2f shift = prev_pts[i] - next_ptsOpt[i]; double n = sqrt(shift.ddot(shift)); if (fabs(n - expected_shift) > thres) { ts->set_failed_test_info(CvTS::FAIL_MISMATCH); return false; } } if (status_Aff[i]) { circle(cnext, next_ptsAff[i], 4, CV_RGB(0, 255, 0)); Point2f shift = prev_pts[i] - next_ptsAff[i]; double n = sqrt(shift.ddot(shift)); if (fabs(n - expected_shift) > thres) { ts->set_failed_test_info(CvTS::FAIL_MISMATCH); return false; } } } /*namedWindow("P"); imshow("P", cprev); namedWindow("N"); imshow("N", cnext); waitKey();*/ return true;}void CV_OptFlowTest::run( int /* start_from */){ if (!runDense(Point(3, 0))) return; if (!runDense(Point(0, 3))) return; //if (!runDense(Point(3, 3))) return; //probably LK works incorrectly in this case. if (!runSparse()) return; ts->set_failed_test_info(CvTS::OK);}CV_OptFlowTest optFlow_test;
