diff --git a/include/tracking.hpp b/include/tracking.hpp index f80004c..328d76c 100644 --- a/include/tracking.hpp +++ b/include/tracking.hpp @@ -4,6 +4,7 @@ #include #include "opencv2/core/core.hpp" +#include "opencv2/opencv.hpp" class Tracker { public: @@ -11,3 +12,19 @@ class Tracker { virtual bool Init(const cv::Mat &frame, const cv::Rect &roi) = 0; virtual cv::Rect Track(const cv::Mat &frame) = 0; }; + + +class MedianFlowTracker : public Tracker{ + private: + double medianFunc(std::vector); + void delElements(std::initializer_list < std::vector*>, std::vector* status = nullptr, std::vector* revErr = nullptr, double value = 0); + public: + bool Init(const cv::Mat &frame, const cv::Rect &roi) override; + cv::Rect Track(const cv::Mat &frame) override; + cv::Rect getRect() { return position_; } + + + protected: + cv::Rect position_; + cv::Mat frame_; +}; \ No newline at end of file diff --git a/samples/tracking_demo.cpp b/samples/tracking_demo.cpp new file mode 100644 index 0000000..fa2b92c --- /dev/null +++ b/samples/tracking_demo.cpp @@ -0,0 +1,64 @@ +#include +#include "detection.hpp" +#include "tracking.hpp" + +using namespace std; +using namespace cv; + +const char* kOptions = +"{ v video | | video to process }" +"{ c camera | | camera to get video from }" +"{ h ? help usage | | print help message }"; + +void CallBackFunc(int event, int x, int y, int flags, void* userdata) +{ + if (event == EVENT_LBUTTONDOWN) + { + static vector points; + points.push_back(Point_(x, y)); + if (points.size() > 1) { + *reinterpret_cast *>(userdata) = Rect2i(points[0], points[1]); + points.clear(); + } + } +} + +int main(int argc, const char** argv) { + CommandLineParser parser(argc, argv, kOptions); + if (parser.get("help")) { + parser.printMessage(); + return 0; + } + Rect_ roi; + if (parser.has("v")) { + Mat frame; + VideoCapture video(parser.get("v")); + MedianFlowTracker tracker; + video.read(frame); + imshow("Result", frame); + cvtColor(frame, frame, CV_BGR2GRAY); + setMouseCallback("Result", CallBackFunc, &roi); + while (true) { + waitKey(1); + if (!roi.empty()) break; + } + tracker.Init(frame, roi); + while (video.read(frame)) { + Mat tmp = frame; + tracker.Track(frame); + cvtColor(frame, frame, CV_BGR2GRAY); + rectangle(tmp, tracker.getRect(), Scalar(250, 150, 10)); + imshow("Result", tmp); + if (waitKey(1) == 27) break; + } + video.release(); + } + else { + cerr << "Error load model"; + return -1; + } + return 0; +} + + + \ No newline at end of file diff --git a/src/tracking.cpp b/src/tracking.cpp index 26cbe06..7472bd1 100644 --- a/src/tracking.cpp +++ b/src/tracking.cpp @@ -7,7 +7,118 @@ using std::shared_ptr; using namespace cv; shared_ptr Tracker::CreateTracker(const string &name) { + + if (name == "median_flow") { + return std::make_shared(); + } + std::cerr << "Failed to create tracker with name '" << name << "'" << std::endl; return nullptr; } + +double MedianFlowTracker::medianFunc(std::vector vec) { + if (vec.empty()) throw - 1; + if (vec.size() % 2 == 0) { + auto median_it1 = vec.begin() + vec.size() / 2 - 1; + auto median_it2 = vec.begin() + vec.size() / 2; + + std::nth_element(vec.begin(), median_it1, vec.end()); + auto p1 = *median_it1; + + std::nth_element(vec.begin(), median_it2, vec.end()); + auto p2 = *median_it2; + + return (p1 + p2) / 2; + + } + else { + const auto median_it = vec.begin() + vec.size() / 2; + std::nth_element(vec.begin(), median_it, vec.end()); + return *median_it; + } +} +void MedianFlowTracker::delElements(std::initializer_list < std::vector*> list, std::vector* status, + std::vector* revErr, double value) { + + int size = status ? status->size() : revErr->size(); + int i = 0; + for (int j = 0; j < size; j++) + if ((status && *(status->begin() + j)) || (revErr && *(revErr->begin() + j) > value)) { + for (auto k = list.begin(); k != list.end(); k++) + ((*k)->begin() + i) = ((*k)->begin() + j); + i++; + } + for (auto k = list.begin(); k != list.end(); k++) + (*k)->resize(i); +} + + +bool MedianFlowTracker::Init(const cv::Mat &frame, const cv::Rect &roi) { + if (frame.empty()) return false; + frame_ = frame; + position_ = roi; + return true; +} +cv::Rect MedianFlowTracker::Track(const cv::Mat &frame) { + Mat obj; + std::vector corners; + (frame(position_)).copyTo(obj); + cvtColor(frame(position_), obj, COLOR_BGR2GRAY); + goodFeaturesToTrack(obj, corners, 100, 0.03, 5); + if (corners.empty()) throw "corners not found"; + std::vector nextPts; + std::vector status; + std::vector err; + Mat gray_frame; + cvtColor(frame, gray_frame, COLOR_BGR2GRAY); + Mat cir = obj.clone(); + for (int i = 0; i < corners.size(); i++) + corners[i] += Point2f(position_.x, position_.y); + calcOpticalFlowPyrLK(frame_, gray_frame, corners, nextPts, status, err); + std::initializer_list < std::vector*> list = { &nextPts, &corners}; + delElements(list, &status); + std::vector revPts; + calcOpticalFlowPyrLK(gray_frame, frame_, nextPts, revPts, status, err); + list = { &nextPts, &revPts, &corners }; + delElements(list, &status); + if (corners.size() < 2) + throw "no corners"; + + std::vector revErr; + for (int j = 0; j < corners.size(); j++) + revErr.emplace_back(norm(corners[j] - revPts[j])); + list = { &nextPts, &revPts, &corners }; + delElements(list, 0, &revErr, medianFunc(revErr)); + + std::vector diffX, diffY; + for (int j = 0; j < corners.size(); j++) { + diffX.emplace_back(nextPts[j].x - revPts[j].x); + diffY.emplace_back(nextPts[j].y - revPts[j].y); + } + + Point2f newPoint(medianFunc(diffX),medianFunc(diffY)); + std::vector coeffs; + + for (int i = 0; i < corners.size(); i++) { + for (int j = i + 1; j < corners.size(); j++) { + double nextNorm = norm(nextPts[i] - nextPts[j]); + double prevNorm = norm(corners[i] - corners[j]); + coeffs.emplace_back(nextNorm / prevNorm); + } + } + double coeff; + try { + coeff = medianFunc(coeffs); + } + catch (int a) { + coeff = 1; + } + + Rect next_position = position_ + Point(newPoint); + next_position.width *= coeff; + next_position.height *= coeff; + position_ = next_position; + frame_ = gray_frame; + return next_position; +} \ No newline at end of file