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

实现Lucas-Kanade光流计算的Delphi类2012-01-15 刘留
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.