video.cpp

#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <iostream>
#include <stdio.h>

using namespace std;
using namespace cv;

#define VECTOR_MIN 2   // the minimum size of a motion vector to consider it
#define VECTOR_MAX 50  // max size
#define REGION_SIZE 10 // size of square region used to calcultate motion vector
#define STEP_SIZE 25   // spacing between points considered for motion vector calc.
#define FRAME_SKIP 1   // spacing between points considered for motion vector calc.

#define LEFT     0
#define UNKNOWN  1
#define RIGHT    2

#define RAD2DEG  180 / 3.14  

std::vector<Mat> LK(const Mat frameT1, const Mat frameT2);
std::vector<Mat> getDerivatives(cv::Mat const& frameT1, cv::Mat const& frameT2);
void LKTracker(const Mat Ix, const Mat Iy, const Mat It, int x, int y, int regionSize, Mat& toReturn);

int DrawVectors(Mat frame, const Mat Vx, const Mat Vy, int scale); // draws vectors and makes a decision on left or right

int main( int argc, const char** argv )
{
  cv::VideoCapture cap;
  if(argc > 1)
  {
    cap.open(string(argv[1]));
  }
  else
  {
    cap.open(CV_CAP_ANY);
  }
  if(!cap.isOpened())
  {
    printf("Error: could not load a camera or video.\n");
  }
  Mat frame;
  Mat framePrev, displayFrame;
  std::vector<Mat> V;
  int frameCount = 0;
  Mat Ix, Iy, It;
  namedWindow("video", 1);
  // namedWindow("Ix", 1);
  // namedWindow("Iy", 1);
  // namedWindow("It", 1);
  // namedWindow("Vx", 1);
  // namedWindow("Vy", 1);
  cap >> frame; // initialise
  framePrev = frame; 
  for(;;)
  {
    waitKey(10);

    cap >> frame;

    if(!frame.data)
    {
      printf("Error: no frame data.\n");
      break;
    }

    displayFrame = frame;
    
    if(frameCount%FRAME_SKIP==0)
    {
      cvtColor(frame, frame, CV_RGB2GRAY);

      
      std::vector<Mat> I = getDerivatives(frame, framePrev);
      Ix = I[0];
      Iy = I[1];
      It = I[2];

      V = LK(frame, framePrev);
      framePrev = frame; 
    }
    frameCount++;


    // plot motion vectors, calculte and display gesture recognition
    DrawVectors(displayFrame, V[0], V[1], 1);
    

    // imshow("Ix", Ix);
    // imshow("Iy", Iy);
    // imshow("It", It);
    // imshow("Vx", V[0]);
    // imshow("Vy", V[1]);
    imshow("video", displayFrame);

  }
}

/**
* @warning frame must be grayscale
*/
std::vector<Mat> getDerivatives(cv::Mat const& frameT1, cv::Mat const& frameT2) {

  int w = frameT1.size().width - 1;
  int h = frameT1.size().height - 1;

  Mat dx, dy, dt;

  dx.create(h, w, frameT1.type());
  dy.create(h, w, frameT1.type());
  dt.create(h, w, frameT1.type());

  // x and y are the wrong way around because of rows, cols differences
  for(int x = 0 ; x < h; x++){ //Window over x
    for(int y = 0; y < w; y++){ //Window over y
      int theDy =            ( ( frameT1.at<uchar>(x, y)   - frameT1.at<uchar>(x+1, y)   ) + 
                             ( frameT1.at<uchar>(x, y+1) - frameT1.at<uchar>(x+1, y+1) ) +
                             ( frameT2.at<uchar>(x, y)   - frameT2.at<uchar>(x+1, y)   ) + 
                             ( frameT2.at<uchar>(x, y+1) - frameT2.at<uchar>(x+1, y+1) )   ) / 4;
      dy.at<uchar>(x, y) = (theDy + 256) / 2;
      int theDx= ( ( frameT1.at<uchar>(x, y)   - frameT1.at<uchar>(x, y+1)   ) + 
                 ( frameT1.at<uchar>(x+1, y) - frameT1.at<uchar>(x+1, y+1) ) +
                 ( frameT2.at<uchar>(x, y)   - frameT2.at<uchar>(x, y+1)   ) + 
                 ( frameT2.at<uchar>(x+1, y) - frameT2.at<uchar>(x+1, y+1) )   ) / 4;
      dx.at<uchar>(x, y) = (theDx + 256) / 2;
      int theDt= ( ( frameT1.at<uchar>(x, y)     - frameT2.at<uchar>(x, y)     ) +
                 ( frameT1.at<uchar>(x+1, y)   - frameT2.at<uchar>(x+1, y)   ) +
                 ( frameT1.at<uchar>(x, y+1)   - frameT2.at<uchar>(x, y+1)   ) +
                 ( frameT1.at<uchar>(x+1, y+1) - frameT2.at<uchar>(x+1, y+1) ) )  / 4;
      dt.at<uchar>(x, y) = (theDt + 256) / 2;
    }
  }

  std::vector<Mat> toReturn;
  toReturn.push_back(dx);
  toReturn.push_back(dy);
  toReturn.push_back(dt);
  return toReturn;
}

void LKTracker(const Mat Ix, const Mat Iy, const Mat It, int x, int y, int regionSize, Mat& toReturn) {
  //cout << "Begin LKTracker" << endl;
  Mat A;
  Mat B;

  A = Mat::zeros(2, 2, CV_64FC1);
  B= Mat::zeros(2, 1, CV_64FC1);

  regionSize--;

  for(int i = x - (regionSize/2); i < x + (regionSize/2); i++) {
    for(int j = y - (regionSize/2); j < y + (regionSize/2); j++) {
      Mat a;
      a.create(2, 2, CV_64FC1);
      a.at<double>(0,0) = Ix.at<uchar>(i, j) * Ix.at<uchar>(i, j);
      a.at<double>(0,1) = Ix.at<uchar>(i, j) * Iy.at<uchar>(i, j);
      a.at<double>(1,0) = a.at<double>(0,1);
      a.at<double>(1,1) = Iy.at<uchar>(i,j) * Iy.at<uchar>(i, j);

      A = A + a;

      Mat b;
      b.create(2, 1, CV_64FC1);
      b.at<double>(0,0) = ( - Ix.at<uchar>(i, j)) * It.at<uchar>(i, j);
      b.at<double>(1,0) = ( - Iy.at<uchar>(i, j)) * It.at<uchar>(i, j);

      B += b;
    }
  }

  
  toReturn = A.inv() * (B);
  //cout << toReturn.size() << endl;
  //cout << "End LKTracker" << endl;


}

std::vector<Mat> LK(const Mat frameT1, const Mat frameT2) {

  Mat Vx;
  Mat Vy;
  Vx = cv::Mat::zeros(frameT1.size(), CV_64FC1);
  Vy = cv::Mat::zeros(frameT1.size(), CV_64FC1);

  Mat Ix, Iy, It;
  std::vector<Mat> I = getDerivatives(frameT1, frameT2);
  Ix = I[0];
  Iy = I[1];
  It = I[2];

  int stepSize = STEP_SIZE;
  for(int x = stepSize; x < frameT1.size().height - stepSize; x+= stepSize) {
    for(int y = stepSize; y < frameT1.size().width - stepSize; y += stepSize) {
     // cout << "x= " << x << "\ty=" << y << endl;
      Mat v;
      LKTracker(Ix, Iy, It, x, y, REGION_SIZE, v);
     // cout << v.size() << endl;
     // cout << "yes" << endl;
      Vx.at<double>(x, y) = v.at<double>(0, 0);
     // cout << "yes2" << endl;
      Vy.at<double>(x, y) = v.at<double>(0, 1);
    }
  }

  

  std::vector<Mat> toReturn;
  toReturn.push_back(Vx);
  toReturn.push_back(Vy);
  return toReturn;
}

int getDirection(double x, double y) {

  if( y > 5) {
    return UNKNOWN; 
  }

  if(x > 1) {
    return RIGHT;
  } else if (x < -1) {
    return LEFT;
  } else {
    return UNKNOWN;
  }
}

// returns LEFT, RIGHT or UNKNOWN
int getMax(int dLeft, int dRight, int dUnknown) {

  if(dLeft + dRight < 10) {
    // not enough data
    return UNKNOWN;
  }
  double ratio = (double)dLeft / (double)dRight;
  if(ratio < 0.9) {
    return LEFT;
  } else if(ratio > 1.1) {
    return RIGHT;
  }

  // if(dLeft > dRight + 5) {
  //   return LEFT;
  // }
  // if(dRight > dLeft + 5) {
  //   return RIGHT;
  // }
  return UNKNOWN;
}

int DrawVectors(Mat frame, const Mat Vx, const Mat Vy, int scale) {
  int directionCount[3];
  directionCount[LEFT] = 0;
  directionCount[UNKNOWN] = 0;
  directionCount[RIGHT] = 0;

  double xSum = 0;

  int lengthScale = 2;
  for(int x = 0 ; x < Vx.size().height; x++){
    for(int y = 0; y < Vx.size().width; y++){
      if(Vx.at<double>(x,y) != 0 && Vy.at<double>(x,y) != 0){
        //I AM A MOTION VECTOR
        double thresholdMin = VECTOR_MIN;
        double thresholdMax = VECTOR_MAX;
        double magnitude = sqrt( (Vx.at<double>(x,y) * Vx.at<double>(x,y)) + (Vy.at<double>(x,y) * Vy.at<double>(x,y)) );

        if(magnitude > thresholdMin && magnitude < thresholdMax) {
          // this point is interesting
          line(frame, Point(scale*y , scale*x), Point(scale*(y + lengthScale*Vy.at<double>(x,y)), 
               scale*(x + lengthScale*Vx.at<double>(x,y))), CV_RGB(0, 255, 0));
          directionCount[  getDirection( Vx.at<double>(x,y) , Vy.at<double>(x,y)) ]++;
          xSum += Vx.at<double>(x, y);
        } else {
          line(frame, Point(scale*y, scale*x), Point(scale*y, scale*x), CV_RGB(255, 0, 0));
        }
      }
    }
  }

  // decide whether left, right or unknown

  ostringstream s;
  switch(getMax(directionCount[LEFT], directionCount[RIGHT], directionCount[UNKNOWN])) {
    case LEFT:
      s << "Left";
      break;
    case RIGHT:
      s << "Right";
      break;
    case UNKNOWN:
      s << "Unknown";
      break;
  }
  putText(frame, s.str(), Point(10, 50), FONT_HERSHEY_SIMPLEX, 1, CV_RGB(255, 0, 255), 1, 8, false ); 
  ostringstream s2;
  s2 << directionCount[LEFT] << "," << directionCount[RIGHT] << "," << directionCount[UNKNOWN] << "; ratio = " << (double)directionCount[LEFT] / (double)directionCount[RIGHT];
  putText(frame, s2.str(), Point(200, 50), FONT_HERSHEY_SIMPLEX, 1, CV_RGB(255, 0, 255), 1, 8, false ); 
  return UNKNOWN;
}