Delphi for realizing Lucas-kanade optical flow calculation

Geo.cra[at]gmail[dot]com
}
Unit opticalflowlk;

Interface

Uses
Math, Windows, sysutils, variants, Classes, Graphics, Unitypes, Colorconv;

Type

TOPTICALFLOWLK = Class
Private
Imageold, Imagenew:ttriplelongintarray;
Imagegray, DX, dy, dxy:tdoublelongintarray;
Eigenvalues:tdoubleextendedarray;
Wbpoint:tdoublebooleanarray;
Height, Width, L, Radiox, Radioy:longint;
Procedure Cornerdetect (Swidth, Sheight:longint; quality:extended);
Procedure Makepyramid (var Images:ttriplelongintarray; Swidth, Sheight, Sl:longint);
Public
Frame:tbitmap;
Features:tsinglepointinfoarray;
Featurecount, Supportcount:longint;
Speed, radio:extended;
Procedure Init (Swidth, Sheight, Sl:longint);
Procedure Initfeatures (FRAMES:TBITMAP);
Procedure Calopticalflowlk (FRAMES:TBITMAP);
destructor Destroy; Override
End

Implementation

Procedure Topticalflowlk.cornerdetect (Swidth, Sheight:longint; quality:extended);
Var
I, J, Fi, Fj:longint;
A, B, C, Sum, minaccept, maxeigenvalue:extended;
Begin
Featurecount: = 0;
{
The following methods are introduced by good Feature to track
J. Shi and C. Tomasi "Good Features to Track", CVPR 94
}
For I: = 1 to SWidth-2 do
for J: = 1 to SHeight-2 do begin
Dx[i, j]: = Imagegray[i-1, j-1] + 2 * imagegray[i-1, J] + imagegray[i-1, j + 1]
-(imagegray[i + 1, j-1] + 2 * imagegray[i + 1, j] + Imagegray[i + 1, j + 1]);
Dy[i, j]: = Imagegray[i-1, J + 1] + 2 * imagegray[i, J + 1] + Imagegray[i + 1, j + 1]
-(Imagegray[i-1, j-1] + 2 * imagegray[i, j-1] + imagegray[i + 1, j-1]);
Dxy[i, j]: = Imagegray[i + 1, j-1] + imagegray[i-1, j + 1]
-(Imagegray[i-1, j-1] + imagegray[i + 1, j + 1]);
End
{To obtain DX, Dy, Dxy of Sobel operator
Dx:
|1 0-1|
0-2|
|1 0-1|
Dy:
|-1-2 -1|
| 0 0 0|
| 1 2 1|
Dxy
|-1 0 1|
| 0 0 0|
| 1 0-1|}
Maxeigenvalue: = 0;
For I: = 2 to SWidth-3 do
for j: = 2 to sHeight-3 do begin
A: = 0; B: = 0; c: = 0;
for fi: = I-1 to i + 1 do
For FJ: = J-1 to J + 1 do begin
A: = A + SQR (Dx[fi, FJ]);
B: = B + Dxy[fi, FJ];
c: = C + sqr (Dy[fi, FJ]);
End
A: = A/2; c: = C/2;
Eigenvalues[i, j]: = (A + c-sqrt (a-c) * (A-C) + b * b));
If Eigenvalues[i, J] > Maxeigenvalue then maxeigenvalue: = Eigenvalues[i, j];
End
{Fetch matrix
|∑dx*dx∑dxy|
m=| |
|∑dxy∑dy*dy|
The eigenvalues
Λ=∑DX*DX +∑dy*dy-((∑dx*dx+∑dy*dy) ^2-4* (∑dx*dx *∑dy*dy-∑dxy *∑DXY)) ^1/2}
Minaccept: = Maxeigenvalue * Quality;
{Set minimum allowable threshold}

For I: = 8 to SWidth-9 do
For J: = 8 to SHeight-9 do
If Eigenvalues[i, J] > Minaccept THEN BEGIN
Wbpoint[i, j]: = true;
INC (Featurecount);
End Else
Wbpoint[i, j]: = false;

For I: = 8 to SWidth-9 do
For J: = 8 to SHeight-9 do
If Wbpoint[i, j] then BEGIN
Sum: = Eigenvalues[i, j];
for fi: = i-8 to i + 8 do begin
For FJ: = j-8 to J + 8 do
If Sqr (fi-i) + SQR (fj-j) <=
if (Eigenvalues[fi, FJ] >= sum) and ((fi <> i) or (FJ <> J)) and (Wbpoint[fi, FJ]) THEN BEGIN
Wbpoint[i, j]: = false;
Dec (Featurecount);
Break
End
If not Wbpoint[i, j] then break;
End
End
{Suppressing false corners with non-maximized suppression}

SetLength (Features, Featurecount); Fi: = 0;
For I: = 8 to SWidth-9 do
For J: = 8 to SHeight-9 do
If Wbpoint[i, j] then BEGIN
FEATURES[FI]. info.x: = i;
FEATURES[FI]. INFO.Y: = j;
FEATURES[FI]. Index: = 0;
Inc. (FI);
End
{Output final point sequence}
End

Procedure Topticalflowlk.init (Swidth, Sheight, Sl:longint);
Begin
Width: = Swidth; Height: = Sheight; L: = SL;
SetLength (Imageold, Width, Height, L);
SetLength (Imagenew, Width, Height, L);
Frame: = tbitmap.create;
Frame.width: = Width; Frame.height: = Height;
Frame.pixelformat: = Pf24bit;
SetLength (Imagegray, Width, Height);
SetLength (eigenvalues, Width, Height);
SetLength (DX, Width, Height);
SetLength (Dy, Width, Height);
SetLength (DXY, Width, Height);
SetLength (Wbpoint, Width, Height);
Featurecount: = 0;
End

Procedure Topticalflowlk.makepyramid (var Images:ttriplelongintarray; Swidth, Sheight, Sl:longint);
Var
I, J, K, II, JJ, Nwidth, nheight, Owidth, Oheight:longint;
Begin
{Generate pyramid image}
Owidth: = Swidth; Oheight: = Sheight;
For k: = 1 to SL-1 do begin
Nwidth: = (owidth + 1) shr 1; Nheight: = (oheight + 1) shr 1;
For I: = 1 to NWidth-2 do begin
II: = i SHL 1;
for J: = 1 to NHeight-2 do begin
JJ: = j SHL 1;
Images[i, J, K]: = (Images[ii, JJ, K-1] SHL 2 +
Images[ii-1, JJ, K-1] SHL 1 + images[ii + 1, JJ, K-1] SHL 1 + images[ii, jj-1, k-1] SHL 1 + images[ii, JJ + 1, K -1] SHL 1 +
Images[ii-1, Jj-1, k-1] + images[ii + 1, jj-1, k-1] + images[ii-1, JJ + 1, k-1] + images[ii + 1, JJ + 1, K- 1] SHR 4;
{Gauss principle, SHL right shift, SHR left shift}
End
End
For I: = 1 to NWidth-2 do begin
II: = i SHL 1;
Images[i, 0, K]: = (images[ii, 0, k-1] SHL 2 +
Images[ii-1, 0, k-1] SHL 1 + images[ii + 1, 0, k-1] SHL 1 + images[ii, 0, k-1] SHL 1 + images[ii, 1, k-1] SHL 1 +
Images[ii-1, 0, k-1] + images[ii + 1, 0, k-1] + images[ii-1, 1, k-1] + images[ii + 1, 1, k-1]) SHR 4;
Images[i, NHeight-1, K]: = (images[ii, oHeight-1, K-1] SHL 2 +
Images[ii-1, OHeight-1, k-1] SHL 1 + images[ii + 1, oHeight-1, k-1] SHL 1 + images[ii, oHeight-2, k-1] SHL 1 + Images[ii, oHeight-1, k-1] SHL 1 +
Images[ii-1, OHeight-2, k-1] + images[ii + 1, oHeight-2, k-1] + images[ii-1, oHeight-1, k-1] + images[ii + 1, OHeight-1, K-1]) SHR 4;
End
{processing the bottom of}
for J: = 1 to NHeight-2 do begin
JJ: = j SHL 1;
Images[0, J, K]: = (images[0, JJ, K-1] SHL 2 +
Images[0, JJ, K-1] SHL 1 + images[1, JJ, K-1] SHL 1 + images[0, jj-1, k-1] SHL 1 + images[0, JJ + 1, k-1] SHL 1 +
Images[0, Jj-1, k-1] + images[1, jj-1, k-1] + images[0, JJ + 1, k-1] + images[1, JJ + 1, k-1] SHR 4;
Images[nwidth-1, J, K]: = (images[owidth-1, JJ, K-1] SHL 2 +
Images[owidth-2, JJ, K-1] SHL 1 + images[owidth-1, JJ, K-1] SHL 1 + images[owidth-1, jj-1, k-1] SHL 1 + imag Es[owidth-1, JJ + 1, k-1] SHL 1 +
Images[owidth-2, Jj-1, k-1] + images[owidth-1, jj-1, k-1] + images[owidth-2, JJ + 1, k-1] + images[owidth -1, JJ + 1, k-1] SHR 4;
End
{Processing left and right sides}
Images[0, 0, K]: = (images[0, 0, k-1] SHL 2 +
Images[0, 0, k-1] SHL 1 + images[1, 0, k-1] SHL 1 + images[0, 0, k-1] SHL 1 + images[0, 1, k-1] SHL 1 +
Images[0, 0, k-1] + images[1, 0, k-1] + images[0, 1, k-1] + images[1, 1, k-1]) SHR 4;
{process left Upper point}
Images[0, NHeight-1, K]: = (images[0, oHeight-1, K-1] SHL 2 +
Images[0, OHeight-1, k-1] SHL 1 + images[1, oHeight-1, k-1] SHL 1 + images[0, oHeight-2, k-1] SHL 1 + images[0 , OHeight-1, k-1] SHL 1 +
Images[0, OHeight-2, k-1] + images[1, oHeight-2, k-1] + images[0, oHeight-1, k-1] + images[1, oHeight-1, K- 1] SHR 4;
{process left lower point}
Images[nwidth-1, 0, K]: = (images[owidth-1, 0, k-1] SHL 2 +
Images[owidth-2, OHeight-1, k-1] SHL 1 + images[owidth-1, oHeight-1, k-1] SHL 1 + images[owidth-1, Oheight- 1, K-1] SHL 1 + images[owidth-1, oHeight-1, k-1] SHL 1 +
Images[owidth-2, OHeight-1, k-1] + images[owidth-1, oHeight-1, k-1] + images[owidth-2, oHeight-1, k-1] + Images[owidth-1, OHeight-1, K-1]) SHR 4;
{Handling the top right point}
Images[nwidth-1, NHeight-1, K]: = (images[owidth-1, oHeight-1, K-1] SHL 2 +
Images[owidth-2, OHeight-1, k-1] SHL 1 + images[owidth-1, oHeight-1, k-1] SHL 1 + images[owidth-1, Oheight- 2, K-1] SHL 1 + images[owidth-1, oHeight-1, k-1] SHL 1 +
Images[owidth-2, OHeight-2, k-1] + images[owidth-1, oHeight-2, k-1] + images[owidth-2, oHeight-1, k-1] + Images[owidth-1, OHeight-1, K-1]) SHR 4;
{Handle lower-right point}
End
End

Procedure Topticalflowlk.initfeatures (FRAMES:TBITMAP);
Var
I, J:longint;
Line:prgbtriple;
Begin
Setstretchbltmode (Frame.Canvas.Handle, Stretch_halftone);
StretchBlt (Frame.Canvas.Handle, 0, 0, Width, Height, Frames.Canvas.Handle, 0, 0, frames.width, Frames.height, srccopy);
For I: = 0 to Height-1 do begin
Line: = Frame.scanline[i];
For J: = 0 to Width-1 do begin
Imageold[j, I, 0]: = (Line^.rgbtblue * one + line^.rgbtgreen * + line^.rgbtred *) div 100;
Imagegray[j, I]: = imageold[j, I, 0];
INC (line);
End
End
{Initializing the first layer of the pyramid image imageold[x,y,0]}
Makepyramid (Imageold, Width, Height, L);
{Generate pyramid image}
Cornerdetect (Width, Height, 0.01);
{Strong Corner detection}
End

Procedure Topticalflowlk.calopticalflowlk (FRAMES:TBITMAP);
Var
I, J, Fi, FJ, K, LL, M, DX, DY, GX, GY, PX, PY, KX, KY, ed, EDC, Nwidth, Nheight:longint;
NX, NY, VX, VY, A, B, C, D, E, F, ik:extended;
Ix, Iy:tdoubleextendedarray;
Line:prgbtriple;
Change:boolean;
Begin
Setstretchbltmode (Frame.Canvas.Handle, Stretch_halftone);
StretchBlt (Frame.Canvas.Handle, 0, 0, Width, Height, Frames.Canvas.Handle, 0, 0, frames.width, Frames.height, srccopy);
For I: = 0 to Height-1 do begin
Line: = Frame.scanline[i];
For J: = 0 to Width-1 do begin
Imagenew[j, I, 0]: = (Line^.rgbtblue * one + line^.rgbtgreen * + line^.rgbtred *) div 100;
INC (line);
End
End
{Initializing the first layer of the pyramid image imagenew[x,y,0]}
Makepyramid (Imagenew, Width, Height, L);
{Generate pyramid image}
SetLength (Ix, 15, 15); SetLength (Iy, 15, 15);
{Request Differential image temp Array}
For M: = 0 to FeatureCount-1 do begin
{See algorithm details:
Jean-yves Bouguet "Pyramidal implementation of the Lucas Kanade Feature Tracker Description of the Algorithm"}
GX: = 0; GY: = 0;
for ll: = L-1 downto 0 DO begin
PX: = Features[m]. info.x SHR ll;
PY: = Features[m]. Info.y SHR ll;
{u-point corresponding to the current pyramid image: U[l]:=u/2^l}
Nwidth: = Width shr ll; Nheight: = Height shr ll;
A: = 0; B: = 0; C: = 0;
For I: = Max (px-7, 1) to min (px + 7, nWidth-2) do
for j: = Max (py-7, 1) to min (py + 7, nHeight-2) DO begin
Fi: = i-px + 7; FJ: = j-py + 7;
Ix[fi, FJ]: = (imageold[i + 1, J, LL]-Imageold[i-1, J, LL])/2;
Iy[fi, FJ]: = (Imageold[i, j + 1, ll]-Imageold[i, J-1, LL])/2;
A: = A + Ix[fi, FJ] * ix[fi, FJ]; B: = B + Ix[fi, FJ] * iy[fi, FJ];
c: = C + Iy[fi, FJ] * iy[fi, FJ];
End
{Calculate the 2-order matrix G:
| IX (x,y) *ix (x,y) IX (x,y) *iy (x,y) |
∑| Ix (x,y) *iy (x,y) Iy (x,y) *iy (x,y) |}
D: = A * C-b * B;
VX: = 0; VY: = 0; DX: = 0; DY: = 0;
If ABS (D) > 1E-8 THEN BEGIN
For k: = 1 do begin
E: = 0; F: = 0;
For I: = Max (px-7, 1) to min (px + 7, nWidth-2) do
for j: = Max (py-7, 1) to min (py + 7, nHeight-2) DO begin
Fi: = i-px + 7; FJ: = j-py + 7;
KX: = i + GX + dx; KY: = j + gy + dy;
If KX < 0 then KX: = 0; If KX > NWidth-1 then KX: = NWidth-1;
If KY < 0 then KY: = 0; If KY > nHeight-1 then KY: = nHeight-1;
Ik: = Imageold[i, J, LL]-Imagenew[kx, KY, LL];
E: = e + Ik * ix[fi, FJ];
F: = f + Ik * iy[fi, FJ];
End
{Calculate 2x1 order matrix B:
| Ik (x,y) *ix (x,y) |
∑| Ik (x,y) *iy (x,y) |}
NX: = (C * e-b * F)/D;
NY: = (B * e-a * F)/(d);
{Calculate Η=g^-1*b}
VX: = VX + NX; VY: = Vy + ny;
DX: = TRUNC (VX); DY: = Trunc (VY);
{Get relative motion vector D}
End
End
GX: = (GX + dx) SHL 1; GY: = (gy + dy) SHL 1;
{Get the next layer of the predictive motion vector g}
End
GX: = GX Div 2; GY: = Gy div 2;
PX: = px + GX; PY: = py + gy;
FEATURES[M]. info.x: = px;
FEATURES[M]. INFO.Y: = py;
FEATURES[M]. Vector.x: = GX;
FEATURES[M]. VECTOR.Y: = Gy;
Then features[m] if (px > Width-1) or (px < 0) or (py > Height-1) or (py < 0). Index: = 1;
{Loss of Feature point processing}
End

For k: = 0 to L-1 do begin
Nwidth: = Width shr k; Nheight: = Height shr k;
For I: = 0 to NWidth-1 do
For J: = 0 to NHeight-1 do
Imageold[i, J, K]: = Imagenew[i, J, K];
End
{Copy J to i}
Repeat
Change: = false;
For I: = 0 to FeatureCount-1 do begin
If Features[i]. Index = 1 Then
For J: = i + 1-FeatureCount-1 do begin
Features[j-1]: = Features[j];
Change: = true;
End
If change then a break;
End
If change then Dec (Featurecount);
Until not change;

SetLength (Features, Featurecount);
{Remove missing feature points}
Speed: = 0; Radio: = 0; Radiox: = 0; Radioy: = 0;
If featurecount > 0 THEN BEGIN
Supportcount: = 0;
For I: = 0 to FeatureCount-1 do
if (features[i). Vector.x <> 0) and (Features[i]. VECTOR.Y <> 0) THEN BEGIN
Radiox: = Radiox + features[i]. vector.x;
Radioy: = Radioy + features[i]. VECTOR.Y;
Speed: = Speed + sqrt (SQR (features[i). vector.x) + SQR (Features[i]. VECTOR.Y));
INC (Supportcount);
End
If supportcount > 0 THEN BEGIN
Speed: = Speed/supportcount; *0.0352778;
Radio: = ArcTan2 (Radioy, Radiox);
End
End
{Calculate average speed and overall orientation}
Frame.Canvas.Pen.Style: = Pssolid;
Frame.Canvas.Pen.Width: = 1;
Frame.Canvas.Pen.Color: = Cllime;
Frame.Canvas.Brush.Style: = Bsclear;
For I: = 0 to FeatureCount-1 do
Frame.Canvas.Ellipse (Features[i]. Info.x-4, Features[i]. Info.y-4, Features[i]. Info.x + 4, features[i]. INFO.Y + 4);
{Identifying feature points with green circles}
Frame.Canvas.Pen.Color: = Clyellow;
For I: = 0 to FeatureCount-1 do begin
Frame.Canvas.MoveTo (Features[i]. Info.x-features[i]. Vector.x, Features[i]. Info.y-features[i]. VECTOR.Y);
Frame.Canvas.LineTo (Features[i]. Info.x, Features[i]. INFO.Y);
End
{The motion vector is represented by a yellow line}
if (Supportcount > 0) and (Speed > 3) THEN BEGIN
Frame.Canvas.Pen.Style: = Pssolid;
Frame.Canvas.Pen.Width: = 6;
Frame.Canvas.Pen.Color: = Clwhite;
Frame.Canvas.Ellipse (width div 2-100, height div 2-100, Width div 2 +, height div 2 + 100);
Frame.Canvas.MoveTo (Width Div 2, Height Div 2);
Frame.Canvas.Pen.Color: = Clblue;
Frame.Canvas.LineTo (Width Div 2 + trunc (* * Cos (Radio)), Height Div 2 + trunc (* Sin (Radio)));
Frame.Canvas.Pen.Style: = Psclear;
Frame.Canvas.Brush.Style: = Bssolid;
Frame.Canvas.Brush.Color: = clred;
Frame.Canvas.Ellipse (Width div 2 + trunc (* * Cos (Radio))-8, Height Div 2 + trunc (* Sin (Radio))-8, Width Div 2 + tr UNC (* Cos (Radio)) + 8, Height Div 2 + trunc (Radio) + 8);
End
End

destructor Topticalflowlk.destroy;
Begin
SetLength (imageold, 0);
SetLength (imagenew, 0);
SetLength (Imagegray, 0);
SetLength (eigenvalues, 0);
SetLength (DX, 0);
SetLength (dy, 0);
SetLength (DXY, 0);
SetLength (wbpoint, 0);
inherited;
End

End.