实现Lucas-Kanade光流计算的Delphi类

核心提示：{作者：刘留参考文献为：Jean-Yves Bouguet "Pyramidal Implementation of the Lucas Kanade Feature Tracker Description of the algorithm"http://www.aivisoft.net/Geo.C

{
作者：刘留
参考文献为：
Jean-Yves Bouguet "Pyramidal Implementation of the Lucas Kanade Feature Tracker Description of the algorithm"
http://www.aivisoft.net/
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.