diff --git a/include/tracking.hpp b/include/tracking.hpp index f80004c..a33653d 100644 --- a/include/tracking.hpp +++ b/include/tracking.hpp @@ -11,3 +11,37 @@ 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 { + public: + virtual bool Init(const cv::Mat &frame, const cv::Rect &roi); + virtual cv::Rect Track(const cv::Mat &frame); + + protected: + cv::Rect position_; + cv::Mat frame_; + cv::Size initial_size_; + float scale_; + + float Median(const std::vector &v) const; + + bool FilterCorners(std::vector &corners, + std::vector &corners_next_frame, + std::vector &status, + std::vector &errors) const; + + bool ComputeMedianShift(const std::vector &corners, + const std::vector &corners_next_frame, + cv::Point2f &shift) const; + + bool ComputePointDistances(const std::vector &corners, + std::vector &dist) const; + + bool ComputeDistScales(const std::vector &dist, + const std::vector &dist_next_frame, + std::vector &scales) const; + + bool ComputeScaleFactor(const std::vector &corners, + const std::vector &corners_next_frame, + float &scale) const; +}; diff --git a/samples/tracking_demo.cpp b/samples/tracking_demo.cpp new file mode 100644 index 0000000..4271435 --- /dev/null +++ b/samples/tracking_demo.cpp @@ -0,0 +1,122 @@ +#include +#include + +#include "opencv2/core.hpp" +#include "opencv2/highgui.hpp" +#include "opencv2/imgproc.hpp" +#include "opencv2/objdetect.hpp" + +#include "tracking.hpp" + +using namespace std; +using namespace cv; + +const char* kAbout = "This is tracking sample application."; + +const char* kOptions = + "{ v video | | video to process }" + "{ c camera | | camera id to capture video from }" + "{ h ? help usage | | print help message }"; + +struct MouseCallbackState { + bool is_selection_started; + bool is_selection_finished; + Point point_first; + Point point_second; +}; + +static void OnMouse(int event, int x, int y, int, void* s) { + MouseCallbackState* state = reinterpret_cast(s); + CV_Assert(state != nullptr); + switch (event) { + case cv::EVENT_LBUTTONDOWN: + state->is_selection_started = true; + state->is_selection_finished = false; + state->point_first = Point(x, y); + break; + case cv::EVENT_LBUTTONUP: + state->is_selection_finished = true; + state->is_selection_started = false; + break; + case cv::EVENT_MOUSEMOVE: + if (state->is_selection_started && !state->is_selection_finished) { + state->point_second = Point(x, y); + } + break; + } +} + +int main(int argc, const char** argv) { + // Parse command line arguments. + CommandLineParser parser(argc, argv, kOptions); + parser.about(kAbout); + + // If help option is given, print help message and exit. + if (parser.get("help")) { + parser.printMessage(); + return 0; + } + + // Load input video. + VideoCapture video; + bool is_live_stream = false; + if (parser.has("video")) { + string video_path = parser.get("video"); + video.open(video_path); + is_live_stream = false; + } + if (parser.has("camera")) { + video.open(parser.get("camera")); + is_live_stream = true; + } + + if (!video.isOpened()) { + cout << "Failed to open video." << endl; + return 0; + } + + const string kWindowName = "video"; + const int kWaitKeyDelay = 20; + const int kEscapeKey = 27; + const Scalar kColorBlue = CV_RGB(0, 0, 255); + const Scalar kColorGreen = CV_RGB(0, 255, 0); + const int kLineThickness = 2; + + namedWindow(kWindowName); + MouseCallbackState mouse_state; + mouse_state.is_selection_started = false; + mouse_state.is_selection_finished = false; + setMouseCallback(kWindowName, OnMouse, &mouse_state); + + Mat frame; + Rect roi; + video >> frame; + imshow(kWindowName, frame); + while (!mouse_state.is_selection_finished) { + Mat frame_with_selection = frame.clone(); + roi = Rect(mouse_state.point_first, mouse_state.point_second); + rectangle(frame_with_selection, roi, kColorGreen, kLineThickness); + imshow(kWindowName, frame_with_selection); + waitKey(kWaitKeyDelay); + if (is_live_stream) { + video >> frame; + } + } + + MedianFlowTracker tracker; + tracker.Init(frame, roi); + video >> frame; + + while (!frame.empty()) { + roi = tracker.Track(frame); + rectangle(frame, roi, kColorBlue, kLineThickness); + imshow(kWindowName, frame); + int key = waitKey(kWaitKeyDelay) & 0x00FF; + if (key == kEscapeKey) { + break; + } + video >> frame; + } + + return 0; +} diff --git a/src/tracking.cpp b/src/tracking.cpp index 26cbe06..b791312 100644 --- a/src/tracking.cpp +++ b/src/tracking.cpp @@ -1,13 +1,187 @@ #include "tracking.hpp" +#include #include +#include "opencv2/opencv.hpp" + using std::string; using std::shared_ptr; +using std::vector; using namespace cv; shared_ptr Tracker::CreateTracker(const string &name) { - std::cerr << "Failed to create tracker with name '" << name << "'" - << std::endl; + if (name == "median_flow") { + return std::make_shared(); + } else { + std::cerr << "Failed to create tracker with name '" << name << "'" + << std::endl; + } return nullptr; } + +bool MedianFlowTracker::Init(const Mat &frame, const Rect &roi) { + if (frame.channels() == 3) { + cvtColor(frame, frame_, CV_BGR2GRAY); + } + Rect image_bounding_rect(Point(0, 0), frame.size()); + if ((image_bounding_rect & roi) == roi) { + position_ = roi; + initial_size_ = roi.size(); + scale_ = 1.0f; + return true; + } else { + std::cerr << "ROI " << roi << " is outside of image."; + } + return false; +} + +float MedianFlowTracker::Median(const vector &v) const { + auto values = v; + size_t middle = values.size() / 2; + std::nth_element(values.begin(), values.begin() + middle, values.end()); + return values[middle]; +} + +bool MedianFlowTracker::FilterCorners(vector &corners, + vector &corners_next_frame, + vector &status, + vector &errors) const { + for (int i = static_cast(status.size()) - 1; i >= 0; i--) { + if (!status[i]) { + status.erase(status.begin() + i); + corners.erase(corners.begin() + i); + corners_next_frame.erase(corners_next_frame.begin() + i); + errors.erase(errors.begin() + i); + } + } + if (corners.empty()) { + return false; + } + vector errors_copy(errors.size()); + std::copy(errors.begin(), errors.end(), errors_copy.begin()); + float median_error = Median(errors_copy); + + for (int i = static_cast(errors.size()) - 1; i >= 0; i--) { + if (errors[i] > median_error) { + errors.erase(errors.begin() + i); + corners.erase(corners.begin() + i); + corners_next_frame.erase(corners_next_frame.begin() + i); + status.erase(status.begin() + i); + } + } + if (corners.empty()) { + return false; + } + + return true; +} + +bool MedianFlowTracker::ComputeMedianShift(const vector &corners, + const vector &nextCorners, + Point2f &shift) const { + vector shifts_x, shifts_y; + for (size_t i = 0; i < corners.size(); ++i) { + shifts_x.emplace_back(nextCorners.at(i).x - corners.at(i).x); + shifts_y.emplace_back(nextCorners.at(i).y - corners.at(i).y); + } + float dx = Median(shifts_x); + float dy = Median(shifts_y); + shift = Point2f(dx, dy); + return true; +} + +bool MedianFlowTracker::ComputePointDistances(const vector &corners, + vector &dist) const { + dist.clear(); + for (int i = 0; i < corners.size(); i++) { + for (int j = i + 1; j < corners.size(); j++) { + dist.push_back(static_cast(cv::norm(corners.at(i) - corners.at(j)))); + } + } + return true; +} + +bool MedianFlowTracker::ComputeDistScales(const vector &dist, + const vector &dist_next_frame, + vector &scales) const { + scales.clear(); + if (dist.size() != dist_next_frame.size()) { + return false; + } + for (size_t i = 0; i < dist.size(); ++i) { + if (dist.at(i) != 0.0f) { + scales.emplace_back(dist_next_frame.at(i) / dist.at(i)); + } + } + if (scales.empty()) { + return false; + } + return true; +} + +bool MedianFlowTracker::ComputeScaleFactor( + const vector &corners, const vector &corners_next_frame, + float &scale) const { + if (corners.size() <= 1 || corners_next_frame.size() <= 1) { + return false; + } + vector dist, dist_next_frame, scales; + ComputePointDistances(corners, dist); + ComputePointDistances(corners_next_frame, dist_next_frame); + ComputeDistScales(dist, dist_next_frame, scales); + scale = Median(scales); + return true; +} + +Rect MedianFlowTracker::Track(const Mat &frame) { + CV_Assert(!frame.empty()); + Mat object = frame_(position_); + vector corners; + + const int kMaxCorners = 100; + const double kQualityLevel = 0.01; + const double kMinDistance = 5.0; + goodFeaturesToTrack(object, corners, kMaxCorners, kQualityLevel, + kMinDistance); + if (corners.empty()) { + std::cout << "Tracked object is lost." << std::endl; + return Rect(); + } + + for (auto &corner : corners) { + corner += Point2f(position_.tl()); + } + + vector corners_next_frame; + vector status; + vector errors; + Mat next_frame = frame.clone(); + if (next_frame.channels() == 3) { + cvtColor(next_frame, next_frame, CV_BGR2GRAY); + } + calcOpticalFlowPyrLK(frame_, next_frame, corners, corners_next_frame, status, + errors); + + if (!FilterCorners(corners, corners_next_frame, status, errors)) { + std::cout << "There are not enough points for tracking." << std::endl; + return Rect(); + } + + Point2f shift; + ComputeMedianShift(corners, corners_next_frame, shift); + + float scale_factor; + if (!ComputeScaleFactor(corners, corners_next_frame, scale_factor)) { + std::cout << "Failed to compute scale factor." << std::endl; + return Rect(); + } + scale_ *= scale_factor; + Rect new_position = Rect(position_.tl() + Point(shift), Size2f(initial_size_) * scale_); + Rect image_bounding_box(Point(0, 0), frame_.size()); + new_position = image_bounding_box & new_position; + + position_ = new_position; + frame_ = next_frame; + return position_; +}