Solution for practice #4

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<float> &v) const;
+
+  bool FilterCorners(std::vector<cv::Point2f> &corners,
+                     std::vector<cv::Point2f> &corners_next_frame,
+                     std::vector<uchar> &status,
+                     std::vector<float> &errors) const;
+
+  bool ComputeMedianShift(const std::vector<cv::Point2f> &corners,
+                          const std::vector<cv::Point2f> &corners_next_frame,
+                          cv::Point2f &shift) const;
+
+  bool ComputePointDistances(const std::vector<cv::Point2f> &corners,
+                             std::vector<float> &dist) const;
+
+  bool ComputeDistScales(const std::vector<float> &dist,
+                         const std::vector<float> &dist_next_frame,
+                         std::vector<float> &scales) const;
+
+  bool ComputeScaleFactor(const std::vector<cv::Point2f> &corners,
+                          const std::vector<cv::Point2f> &corners_next_frame,
+                          float &scale) const;
+};

samples/tracking_demo.cpp

@@ -0,0 +1,122 @@
+#include <iostream>
+#include <string>
+
+#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        | <none> | video to process                }"
+    "{ c camera       | <none> | 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<MouseCallbackState*>(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<bool>("help")) {
+    parser.printMessage();
+    return 0;
+  }
+
+  // Load input video.
+  VideoCapture video;
+  bool is_live_stream = false;
+  if (parser.has("video")) {
+    string video_path = parser.get<string>("video");
+    video.open(video_path);
+    is_live_stream = false;
+  }
+  if (parser.has("camera")) {
+    video.open(parser.get<int>("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;
+}

src/tracking.cpp

@@ -1,13 +1,187 @@
 #include "tracking.hpp"
 
+#include <algorithm>
 #include <iostream>
 
+#include "opencv2/opencv.hpp"
+
 using std::string;
 using std::shared_ptr;
+using std::vector;
 using namespace cv;
 
 shared_ptr<Tracker> Tracker::CreateTracker(const string &name) {
-  std::cerr << "Failed to create tracker with name '" << name << "'"
-            << std::endl;
+  if (name == "median_flow") {
+    return std::make_shared<MedianFlowTracker>();
+  } 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<float> &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<Point2f> &corners,
+                                      vector<Point2f> &corners_next_frame,
+                                      vector<uchar> &status,
+                                      vector<float> &errors) const {
+  for (int i = static_cast<int>(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<float> errors_copy(errors.size());
+  std::copy(errors.begin(), errors.end(), errors_copy.begin());
+  float median_error = Median(errors_copy);
+
+  for (int i = static_cast<int>(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<Point2f> &corners,
+                                           const vector<Point2f> &nextCorners,
+                                           Point2f &shift) const {
+  vector<float> 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<Point2f> &corners,
+                                              vector<float> &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<float>(cv::norm(corners.at(i) - corners.at(j))));
+    }
+  }
+  return true;
+}
+
+bool MedianFlowTracker::ComputeDistScales(const vector<float> &dist,
+                                          const vector<float> &dist_next_frame,
+                                          vector<float> &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<Point2f> &corners, const vector<Point2f> &corners_next_frame,
+    float &scale) const {
+  if (corners.size() <= 1 || corners_next_frame.size() <= 1) {
+    return false;
+  }
+  vector<float> 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<Point2f> 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<Point2f> corners_next_frame;
+  vector<uchar> status;
+  vector<float> 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_;
+}