#include <opencv2/opencv.hpp>

int main(int argc,char**argv)
{
  IplImage *image1=cvLoadImage("cube.png"); 
  IplImage *image2=cvLoadImage("cube2.png"); 
  if ((!image1)||(!image2)) return EXIT_FAILURE;

  CvSize winsize=cvSize(15,15);
  CvSize imsize=cvGetSize(image1);
  CvTermCriteria crit=cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03);
  IplImage *gray1=cvCreateImage(imsize,IPL_DEPTH_8U,1);
  cvCvtColor(image1,gray1,CV_BGR2GRAY);
  IplImage *gray2=cvCreateImage(imsize,IPL_DEPTH_8U,1);
  cvCvtColor(image2,gray2,CV_BGR2GRAY);
  IplImage *imflow=cvCreateImage(imsize,IPL_DEPTH_8U,3);

  IplImage *eig=cvCreateImage(imsize,IPL_DEPTH_32F,1);
  IplImage *tmp=cvCreateImage(imsize,IPL_DEPTH_32F,1);

  int count=500;
  CvPoint2D32f *points1=(CvPoint2D32f*)cvAlloc(count*sizeof(CvPoint2D32f));
  CvPoint2D32f *points2=(CvPoint2D32f*)cvAlloc(count*sizeof(CvPoint2D32f));

  cvGoodFeaturesToTrack(gray1,eig,tmp,points1,&count,0.05,5,0,3,0,0.04);
  cvFindCornerSubPix(gray1,points1,count,winsize,cvSize(-1,-1),crit);

  cvReleaseImage(&eig);
  cvReleaseImage(&tmp);

  char features_found[500]; 

	cvCalcOpticalFlowPyrLK(gray1,gray2,NULL,NULL,points1,points2,count,winsize,5,features_found,NULL,crit,0);

  cvAnd(image1,image2,imflow);
  cvNamedWindow("features");
  CvRNG rng=cvRNG(-1);
  for( int i=0; i<count; i++)
  {
    if (!features_found[i]) continue;
		CvPoint p0 = cvPoint( cvRound( points1[i].x ), cvRound( points1[i].y ) );
		CvPoint p1 = cvPoint( cvRound( points2[i].x ), cvRound( points2[i].y ) );
		cvLine(imflow,p0,p1,CV_RGB(cvRandInt(&rng)%255,cvRandInt(&rng)%255,cvRandInt(&rng)%255),1);
	}

  cvSaveImage("out.png",imflow);

  cvFree(&points1);
  cvFree(&points2);
  cvReleaseImage(&image1);
  cvReleaseImage(&gray1);
  cvReleaseImage(&image2);
  cvReleaseImage(&gray2);
  cvReleaseImage(&imflow);
  return EXIT_SUCCESS;
}
