#include <iostream>

#include <vector>

#include <opencv2/core/core.hpp>

#include <opencv2/video/tracking.hpp>

#include <opencv2/imgproc/imgproc.hpp>

#include <opencv2/highgui/highgui.hpp>

int main(void)

{

  // Load the prev image

  cv::Mat prev = cv::imread("blocks1.png", 1);

  if(prev.empty()) return -1; 

  // Load the next image

  cv::Mat next = cv::imread("blocks2.png", 1);

  if(next.empty()) return -1; 

  std::vector<cv::Point2f> prev_pts;

  std::vector<cv::Point2f> next_pts;

  // Initialize

  cv::Size flowSize(30,30);

  cv::Point2f center = cv::Point(prev.cols/2., prev.rows/2.);

  for(int i=0; i<flowSize.width; ++i) {

    for(int j=0; j<flowSize.width; ++j) {

      cv::Point2f p(i*float(prev.cols)/(flowSize.width-1), 

                    j*float(prev.rows)/(flowSize.height-1));

      prev_pts.push_back((p-center)*0.9f+center);

    }

  }

  cv::Mat status, error;

  // Calc optical flows.

  cv::calcOpticalFlowPyrLK(prev, next, prev_pts, next_pts, status, error);

  // Show results.

  // If you need necessary, please remove the comment.

  /*

  std::vector<cv::Point2f>::const_iterator p = prev_pts.begin();

  std::vector<cv::Point2f>::const_iterator n = next_pts.begin();

  for(; n!=next_pts.end(); ++n,++p) {

    cv::line(prev, *p, *n, cv::Scalar(150,0,0),2);

  }

  cv::namedWindow("optical flow", CV_WINDOW_AUTOSIZE|CV_WINDOW_FREERATIO);

  cv::imshow("optical flow", prev);

  cv::waitKey(0);

  */

  return 0;

  // The heap error occurs after the end of main function.

}