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;
{
下面采用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;
{求取Sobel算子的Dx, Dy, Dxy
Dx:
|1 0 -1|
|2 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;
{求取矩陣
|∑Dx*Dx ∑Dxy|
M=| |
|∑Dxy ∑Dy*Dy|
的特征值
λ= ∑Dx*Dx + ∑Dy*Dy - ((∑Dx*Dx+∑Dy*Dy)^2-4*(∑Dx*Dx * ∑Dy*Dy - ∑Dxy * ∑Dxy))^1/2}
MinAccept := MaxEigenvalue * Quality;
{設置最小允許閥值}
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) <= 64 then
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;
{用非最大化抑制來抑制假角點}
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;
{輸出最終的點序列}
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
{生成金字塔圖像}
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;
{高斯原則，shl右移位，shr左移位}
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;
{處理上下邊}
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 + Images[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;
{處理左右邊}
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;
{處理左上點}
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;
{處理左下點}
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;
{處理右上點}
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;
{處理右下點}
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 * 11 + Line^.rgbtGreen * 59 + Line^.rgbtRed * 30) div 100;
ImageGray[j, i] := ImageOld[j, i, 0];
Inc(Line);
end;
end;
{初始化金字塔圖像第一層ImageOld[x,y,0]}
MakePyramid(ImageOld, Width, Height, L);
{生成金字塔圖像}
CornerDetect(Width, Height, 0.01);
{進行強角點檢測}
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 * 11 + Line^.rgbtGreen * 59 + Line^.rgbtRed * 30) div 100;
Inc(Line);
end;
end;
{初始化金字塔圖像第一層ImageNew[x,y,0]}
MakePyramid(ImageNew, Width, Height, L);
{生成金字塔圖像}
setlength(Ix, 15, 15); setlength(Iy, 15, 15);
{申請差分圖像臨時數組}
for m := 0 to FeatureCount - 1 do begin
{算法細節見:
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點：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;
{計算2階矩陣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 to 10 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;
{計算2x1階矩陣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);
{計算η=G^-1*b}
vx := vx + nx; vy := vy + ny;
dx := trunc(vx); dy := trunc(vy);
{得到相對運動向量d}
end;
end;
gx := (gx + dx) shl 1; gy := (gy + dy) shl 1;
{得到下一層的預測運動向量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;
if (px > Width - 1) or (px < 0) or (py > Height - 1) or (py < 0) then Features[m].Index := 1;
{失去特征點處理}
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;
{復制J到I}
repeat
Change := false;
for i := 0 to FeatureCount - 1 do begin
if Features[i].Index = 1 then
for j := i + 1 to FeatureCount - 1 do begin
Features[j - 1] := Features[j];
Change := true;
end;
if Change then break;
end;
if Change then Dec(FeatureCount);
until not Change;
setlength(Features, FeatureCount);
{刪除失去的特征點}
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;
{計算平均速度和整體方向}
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);
{用綠圈標識特征點}
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;
{用黃色線條表示運動向量}
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 + 100, 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(90 * Cos(Radio)), Height div 2 + trunc(90 * Sin(Radio)));
Frame.Canvas.Pen.Style := psClear;
Frame.Canvas.Brush.Style := bsSolid;
Frame.Canvas.Brush.Color := clRed;
Frame.Canvas.Ellipse(Width div 2 + trunc(90 * Cos(Radio)) - 8, Height div 2 + trunc(90 * Sin(Radio)) - 8, Width div 2 + trunc(90 * Cos(Radio)) + 8, Height div 2 + trunc(90 * Sin(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.
