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

【 tulaoshi.com - 编程语言 】

　　{
　　作者：刘留
　　参考文献为：
　　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.