Simple implementation of the Lucas Kanade optical flow algorithm, including a coarse-to-fine approach and a visualization of the movement detected. See README below for instructions

lucas_kanade.cpp

// Take two pictures, assume one is a translation of the other, find the translation vector

#include <iostream>
#include <cmath>
#include <Winval.h>

using namespace std;
#include <HCam.h>

#include <HGraf.h>

const int numdepths = 7;

int main(int nargs, const char ** args){
  const char* pattern;
  if(nargs >1){
    pattern = args[1];
  }

  HCam cam(640, 480, "/dev/video0", HCAM_MODE_YUYV);
  Winval ww(640, 480);

  int w =640, h = 480;
  unsigned char * image1 = new unsigned char[w*h*4];
  
  unsigned char * image2 = new unsigned char[w*h*4];
  cam.capture_image(image2);
  
  unsigned char * d = 0;

  unsigned char* viso = new unsigned char[w*h*4];
  for(int i = 0; i< w*h*4; i++){
    viso[i] = 0;
  }

  Vector2 totalMove(0,0);
  unsigned char** pyramid2 = new unsigned char*[numdepths];
  

  unsigned char** pyramid = new unsigned char*[numdepths];
  int** grpyramidx = new int*[numdepths];
  int** grpyramidy = new int*[numdepths];
  for(int i = 0; i< numdepths;i++){
    pyramid2[i] = new unsigned char[(w>>i)*(h>>i)];
    pyramid[i] = new unsigned char[(w>>i)*(h>>i)];

    
    grpyramidx[i] = new int[(w>>i)*(h>>i)];
    grpyramidy[i] = new int[(w>>i)*(h>>i)];
  }
  
  while(true){
    d = image1;
    image1 = image2;
    image2 = d;
    cam.capture_image(image2);
    
    if(!image1 || !w){
      cout<<"No image"<<endl;
      return 0;
    }

    
    for(int i= 0; i< h;i ++){
      for(int j = 0; j < w; j++){
	pyramid[0][i*w + j] = (unsigned char)((image1[4*(i*w + j)]*2 + image1[4*(i*w + j) + 1]*3 + image1[4*(i*w + j) + 2])/6);
      }
    }

    int kernel[3][3] = {{1, 2, 1},
			{2, 4, 2},
			{1, 2, 1}};

    //Construct for each depth (with blurring)
    int pcw = w;
    for(int k = 1; k < numdepths; k++){
    
      int cw = w >> k;
      int ch = h >> k;
      for(int i = 0; i< ch; i++){
      
	for(int j = 0; j < cw; j++){
	  int sum = 0;
	  for(int u = -1; u < 2; u++){
	    for(int v = -1; v < 2; v++){
	      int nu =u;
	      int nv = v;
	      if(i == 0){
		nu++;
	      }
	      if(j == 0){
		nv++;
	      }

	      sum += kernel[u + 1][v + 1]*(int)pyramid[k-1][(i*2 + nu)*pcw + j*2 + nv];
	    }
	  }

	  pyramid[k][i*cw + j] = (unsigned char)(sum/16);

	}

      
      }
      pcw = cw;
    }

    //Calculate gradient for each depth
    for(int k = 0;k < numdepths; k++){
      int cw = w>>k, ch = h>>k;
      for(int i = 0; i< ch; i++){
	for(int j = 0; j < cw; j++){
	  if(i ==0 || i == ch - 1 || j == 0 || j == cw - 1){
	    grpyramidx[k][i*cw + j] = 0;
	    grpyramidy[k][i*cw + j] = 0;
	  }else{
	    grpyramidx[k][i*cw + j] = -pyramid[k][(i - 1)*cw + (j-1)] + pyramid[k][(i - 1)*cw + (j+1)]
	      + -2*pyramid[k][(i)*cw + (j - 1)] + 2*pyramid[k][(i)*cw + (j + 1)]
	      + -pyramid[k][(i + 1)*cw + (j - 1)] + pyramid[k][(i + 1)*cw + (j + 1)];

	    grpyramidy[k][i*cw + j] = -pyramid[k][(i - 1)*cw + (j -1)] - 2*pyramid[k][(i - 1)*cw + (j)] -pyramid[k][(i - 1)*cw + (j+ 1)] +
	      pyramid[k][(i + 1)*cw + (j - 1)] + 2*pyramid[k][(i + 1)*cw + (j)] + pyramid[k][(i + 1)*cw + (j + 1)];
	  }
	}
      }
    }


    //Create pyramid for second image

    for(int i= 0; i< h;i ++){
      for(int j = 0; j < w; j++){
	pyramid2[0][i*w + j] = (unsigned char)((image2[4*(i*w + j)]*2 + image2[4*(i*w + j) + 1]*3 + image2[4*(i*w + j) + 2])/6);
      }
    }
    
    pcw = w;
    for(int k = 1; k < numdepths; k++){

      int cw = w>>k;
      int ch = h>>k;

      for(int i = 0; i< ch;i++){
	for(int j = 0; j < cw;j++){
	  int sum = 0;
	  for(int u = -1; u < 2; u++){
	    for(int v = -1; v < 2; v++){
	      int nu =u;
	      int nv = v;
	      if(i == 0){
		nu++;
	      }
	      if(j == 0){
		nv++;
	      }

	      sum += kernel[u + 1][v + 1]*(int)pyramid2[k-1][(i*2 + nu)*pcw + j*2 + nv];
	    }
	  }

	  pyramid2[k][i*cw + j] = (unsigned char)(sum/16);
	}
      }
      pcw = cw;
    }

    //Compare, decide movement

    Vector2 mv(0,0);

    int num_iter = 5;
  
    for(int k= numdepths-1; k >=0; k--){
      mv = mv*2;
      int cw = w>>k;
      int ch = h>>k;
      for(int p = 0; p < num_iter; p++){
      
	Vector2 sum(0,0);
	float dersum = 0;
	//int count = 0;
	for(int i = 0; i < ch; i++){
	  for(int j = 0; j < cw; j++){
	    int nx = j + (int)mv.x;
	    int ny = i + (int)mv.y;
	    if( ny >= ch || ny < 0 || nx >= cw || nx < 0)
	      continue;
	    sum = sum + Vector2( grpyramidx[k][(ny)*cw + nx]*(pyramid2[k][i*cw + j] - pyramid[k][(ny)*cw + nx]),
				 grpyramidy[k][(ny)*cw + nx]*(pyramid2[k][i*cw + j] - pyramid[k][(ny)*cw + nx]));
	    dersum += grpyramidx[k][ny*cw + nx]*grpyramidx[k][ny*cw + nx] +
	      grpyramidy[k][ny*cw + nx]*grpyramidy[k][ny*cw + nx];
	  
	  }
	}
      
	Vector2 u = sum/dersum;
    
	mv = (u + mv);
      }
    }

    

    totalMove = totalMove + mv;

    for(int i = 0; i< w*h*4; i++){
      viso[i] = 0;
    }
    hg::drawLineSafe(viso,w,h, w/2 + totalMove.x/2 - 10, h/2 + totalMove.y/2 - 10, w/2 + totalMove.x/2 + 10, h/2 + totalMove.y/2 + 10, 0xFF0000);
    hg::drawLineSafe(viso, w, h, w/2 +totalMove.x/2 - 10, h/2 + totalMove.y/2 + 10, w/2 + totalMove.x/2 + 10, h/2 + totalMove.y/2 - 10, 0xFF0000);
    
    //cout<<"Last move was "<<mv.str()<<", in total, we have moved "<<totalMove.str()<<endl;
    ww.flushEvents();
    if(ww.isKeyPressed(WK_ESC))
      break;
    ww.drawBuffer(viso, w, h);
  }

  return 0;
  
}

Lucas_Kanade_README.md

Lucas Kanade implementation

This is a simple demonstration of the Lucas-Kanade algorithm. It's not very readable, and I'm sorry for it. I never thought it would find its way onto the webs.

Depends on HConLib. To compile, you need to clone and build HConLib (running build.sh in the root of HConLib is usually sufficient). Then, use the line g++ lucas_kanade.cpp -I /path/to/HConLib/include/ -L /path/to/HConLib/lib/ -l HCam -l Winval -l HGraf -l FlatAlg -l X11 -std=c++11 -o lucas_kanade -O3 and you should be good