imclearboder matlab,Lucas

Lucas-Kanade跟蹤算法是視覺跟蹤中一個很經典的基于點的逐幀跟蹤算法。起初這個算法是用來求解stero matching1的，后來經過Carlo Tomasi2和Jianbo Shi3等人的發展漸趨成熟。Jianbo Shi提出了一種篩選跟蹤點特征的方法，使得特征的跟蹤更可靠。Jean-Yves Bouguet4詳細闡述了如何采用金字塔方式實現LK算法以處理兩幀之間特征點位移較大的情況。

問題闡述

首先我們來看一下我們要解決的問題是什么？LK算法是基于特征點的跟蹤，而這里的特征點就是每個點對應的一個小窗口圖像塊，LK所要解決的是求解連續兩幀圖像相同特征點的位移問題。這里我們假設I和J為連續兩幀圖像，其(x,y)點的灰度值分別對應I(x,y),J(x,y)。設u=[ux,uy]T是圖像I上一點，LK算法的目標是在圖像J找到一點v=u+d=[ux+dx,uy+dy]T使得點I(u)和點J(v)是同一個位置。為了求解這樣的點，LK求解這兩個點對應的小窗口內像素的相似度。設ωx和ωy分別是點左右擴展的窗口范圍，這樣我們可以定義如下residual function為

(d)=(dx,dy)=∑x=uxωxux+ωx∑y=uyωyuy+ωy(I(x,y)J(x+dx,y+dy))2

窗口大小為(2ωx+1)×(2ωy+1),通常情況下ωx和ωy的值為2,3,4,5,6,7。

標準LK算法

針對上述最優化問題，求解方法是求解(d)關于向量d的偏導使其等于0,即

(d)d|d=dopt=[00]

這樣可以推到出其偏導結果：

(d)d=2∑x=uxωxux+ωx∑y=uyωyuy+ωy(I(x,y)J(x+dx,y+dy))J(x+dx,y+dy)d=2∑x=uxωxux+ωx∑y=uyωyuy+ωy(I(x,y)J(x+dx,y+dy))[JxJy]

利用泰勒級數展開J(x+dx,y+dy)得,

J(x+dx,y+dy)=J(x,y)+(dxx+dyy)J(x,y)+12!(dxx+dyy)2J(x,y)+...≈J(x,y)+(dxx+dyy)J(x,y)≈J(x,y)+[JxJy]d

這樣得出residual function為，

(d)d≈2∑x=uxωxux+ωx∑y=uyωyuy+ωy(I(x,y)J(x,y)[JxJy]d)[JxJy]

這里關于圖像J(x,y)的偏導可以通過求解I(x,y)的偏導近似計算。設

▽I=[IxIy]=[JxJy]T,

δI=I(x,y)J(x,y) 這樣residual function變為

12(d)d≈∑x=uxωxux+ωx∑y=uyωyuy+ωy(▽ITdδI)▽IT≈∑x=uxωxux+ωx∑y=uyωyuy+ωy(▽ITd▽ITδI▽IT)

等式兩邊取倒置

12[(d)d]T≈∑x=uxωxux+ωx∑y=uyωyuy+ωy(▽IdT▽IδI▽I)≈∑x=uxωxux+ωx∑y=uyωyuy+ωy(▽I▽ITdδI▽I)≈∑x=uxωxux+ωx∑y=uyωyuy+ωy([I2xIxIyIxIyI2y]d[δIIxδIIy])

我們用簡單符號替代其中的兩個部分，分別設

Gb=∑x=uxωxux+ωx∑y=uyωyuy+ωy[I2xIxIyIxIyI2y]=∑x=uxωxux+ωx∑y=uyωyuy+ωy[δIIxδIIy]

現在residual function變成了，

12[(d)d]T≈Gdb

使上式等于0,得出位移d為，

d=G1b

這里必須保證G是可逆的，也就是保證圖像I(x,y)在x和y方向上的梯度值必須不是0。

以上便是基本的LK算法的推導過程，具體實現的時候需要多次的迭代才能得到一個較準確的點的位移矢量，類似牛頓-拉弗森方法(Newton-Raphson method)的迭代過程，這是一個逐漸趨近最優值的過程。下面詳細介紹迭代的過程，針對第k(k1)次迭代：

設第k1次迭代的位移dk1=[dk1x,dk1y]，則我們利用第k1次迭代的位移作為第k次迭代位移的初始化值，即當前次迭代的J(x,y)變為

J(x,y)=J(x+dk1x,y+dk1y)

residual function變為

(d)=(dx,dy)=∑x=uxωxux+ωx∑y=uyωyuy+ωy(I(x,y)J(x+dkx,y+dky))2

通過一次標準的LK算法，得出第k次的位移

dk=G1bk

這里我們發現每次迭代中，G是不變的，通過I(x,y)計算，唯一變化的是b，每次迭代圖像J(x,y)對應的窗口都會向所要求的位置點靠近一點點(即上一次迭代的位移作為初始化)，而b的計算與J(x,y)有關，所以每次迭代都會發生變化，這樣每次迭代需要計算的就只有b。

假設進行了K次迭代后收斂，最終位移的結果為

d=∑k=1Kdk

對于第一次迭代其對應的初始化位移為：

d0=[00]T

但是上述推導的一個基本假設是點特征的位移是很小的，這樣才能滿足泰勒展開式中只保留前兩項的近似操作。而為了能處理較大的位移情況，則需要基于圖像金字塔在不同分辨率的圖層下進行跟蹤。

圖像金字塔跟蹤

首先舉一個簡單的例子，比如知道一個點前后兩幅圖像的位移為16個像素，這么大的位移直接使用標準LK算法是很難計算出來結果的，而如果在圖像分辨率降低到原來一半后，其位移就變為8個像素，再降低一半，則為4個像素，如果金字塔的層數是3,則在最底層，點的位移只有兩個像素，這樣就滿足了小位移的假設。這樣首先在最底層進行標準LK算法，得出一個位移后乘以2作為上一層的初始位移，再進行標準LK算法，以此類推，最終得到點的位移。

設圖像金字塔層數為L=0,1,2...Lm,跟蹤是從圖像金字塔的最底層Lm開始的，對于圖像從第L+1層到L層的跟蹤流程，和標準LK算法的迭代有點類似，第L層的初始化位置是基于第L+1層計算出來的。

設gL=[gLxgLy]T是第L層的初始化位移，它是通過第L+1層的位移計算得到的。這樣第L層的residual function就變成了

L(dL)=(dLx,dLy)=∑x=uLxωxuLx+ωx∑x=uLyωyuLy+ωy(IL(x,y)JL(x+gLx+dLx,y+gLy+dLy))2

從上式可以看出，第L層的JL(x,y)由于有了gL作為初始化位置使得要求解的位移dL變得很小，也就很適合用標準的LK算法計算了。

gL的計算是通過第L+1層的位移和初始化位置計算的，

gL=2(gL+1+dL+1)

對于最底層的初始化位置設為，

gLm=[00]T

最后得出點的位移為，

d0=g0+d0

圖像金字塔的構建是通過首先對上一層圖像進行去邊緣濾波，然后下采樣得到的，具體的實現參考下面章節。

OpenCV代碼實現分析

這個算法的實現主要分為三個重要的部分：圖像金字塔的構建，圖像梯度圖的計算，標準LK算法的迭代。

圖像金字塔的構建

OpenCV是通過下采樣和去邊緣濾波器兩個流程生成的多分辨率的圖像金字塔，當然為了程序的優化，這兩個流程是同時進行的。濾波器采用的kernel是

12561464141624164624362464162416414641

程序實現的核心函數是

1void pyrDown(InputArray src, OutputArray dst, const Size& dstsize=Size(), int borderType=BORDER_DEFAULT )

其OpenCV的源代碼如下(OpenCV-2.4.8/video/pyramids.cpp/line:187)。這個函數實現的大體思路是以目標圖像的長寬為基準同時實現對源圖像的去邊緣濾波以及下采樣操作。因為圖像在濾波后還要做下采樣，如果這兩步驟是分開做的話，前面濾波到的像素就會額外計算了一半下采樣后根本不需要的像素，浪費了計算，所以這里僅僅是濾波了下采樣中保留下來的像素。

濾波器的實現最直接的想法就是每次計算所有窗口里面的像素值然后求均值，但這樣做會重復計算前后兩行重疊的部分像素和，代碼效率并不高，實際上代碼中的實現是首先計算每一個像素對應的前后兩行的行和，然后存儲下這5個行和，每行所有像素計算完所有5個行和并存儲好后，再用一個for循環求和每個像素的5個行和，這樣就可以避免重復計算前后兩行重復的行和而提高了效率。如下圖所示，中間紅色像素對應窗口為1-5行，藍色像素對應窗口為2-6行，其中其中2-5行的和都是重復的，不需要重復計算。

pyrDown實現示意圖

這里有幾個需要解釋的地方：

第一，代碼第54行的for循環實現的是求解每個元素對應的各個行和。這里它采用了一種循環存取機制。例如，當計算目標圖像第k行像素(即源圖像第2k行像素)時，其需要求解的行和分別是對應源圖像上的2k-2，2k-1，2k，2k+1，2k+2行，這時假設內存中是按照順序方式存儲的，當計算目標圖像k+1行像素(即源圖像第2k+2行像素)時，我們只需要再計算2k+3和2k+4的行和并且存儲在本來存儲2k-2和2k-1行的行和的內存中，這樣的計算和存儲開銷是最小的。這樣說可能有點抽象，如下圖所示，左邊是計算目標圖像第k行像素時，數組中5個元素存儲的內容，右邊是計算目標圖像k+1行像素存儲的內容，僅僅是把原來存儲2k-2和2k-1行的元素替換成新計算出來的2k+3和2k+4行的行和，這樣在訪問這些行和時，順序就會發生一定的變化，由左邊的1,2,3,4,5變成右邊的4,5,1,2,3。這樣在計算k+2行像素時，只需把原來存儲2k-2和2k-1的內存替換為2k+3和2k+4的行和即可，然后依次類推。查看程序第57行WT* row = buf + ((sy - sy0) % PD_SZ)*bufstep;，采用的就是這種循環存儲方法。那這樣在取這些存在數組中的行和是，順序也是對應的順序，因為每個行和要乘以的權重不一，自然順序不能錯。順序的計算方法查看程序第122行rows[k] = buf + ((y*2 - PD_SZ/2 + k - sy0) % PD_SZ)*bufstep;

存儲示意圖

第二，程序在處理邊緣問題是采用了borderInterpolate這個函數，主要涉及對稱邊緣圖像或直接復制邊緣圖像等方式添加虛擬邊緣。當boderType = BORDER_REPLICATE時，是簡單的復制邊緣圖像，當boderType = BORDER_REFLECT時，就是以邊緣為中心對稱復制邊緣里層的圖像。而pyrDown采用的就是這種邊緣處理方式。

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130template void

pyrDown_( const Mat& _src, Mat& _dst, int borderType )

{

//濾波器的窗口大小

const int PD_SZ = 5;

typedef typename CastOp::type1 WT;

typedef typename CastOp::rtype T;

CV_Assert( !_src.empty() );

Size ssize = _src.size(), dsize = _dst.size();

int cn = _src.channels();

int bufstep = (int)alignSize(dsize.width*cn, 16);

AutoBuffer _buf(bufstep*PD_SZ + 16);

WT* buf = alignPtr((WT*)_buf, 16);

//處理左右邊界

int tabL[CV_CN_MAX*(PD_SZ+2)], tabR[CV_CN_MAX*(PD_SZ+2)];

AutoBuffer _tabM(dsize.width*cn);

int* tabM = _tabM;

//存儲PD_SZ行的和，用于第二次循環列的求和

WT* rows[PD_SZ];

CastOp castOp;

VecOp vecOp;

CV_Assert( std::abs(dsize.width*2 - ssize.width) <= 2 &&

std::abs(dsize.height*2 - ssize.height) <= 2 );

int k, x, sy0 = -PD_SZ/2, sy = sy0, width0 = std::min((ssize.width-PD_SZ/2-1)/2 + 1, dsize.width);

for( x = 0; x <= PD_SZ+1; x++ )

{

int sx0 = borderInterpolate(x - PD_SZ/2, ssize.width, borderType)*cn;

int sx1 = borderInterpolate(x + width0*2 - PD_SZ/2, ssize.width, borderType)*cn;

for( k = 0; k < cn; k++ )

{

tabL[x*cn + k] = sx0 + k;

tabR[x*cn + k] = sx1 + k;

}

ssize.width *= cn;

dsize.width *= cn;

width0 *= cn;

for( x = 0; x < dsize.width; x++ )

tabM[x] = (x/cn)*2*cn + x % cn;

//基于目標圖像的高度

for( int y = 0; y < dsize.height; y++ )

{

T* dst = (T*)(_dst.data + _dst.step*y);

WT *row0, *row1, *row2, *row3, *row4;

// fill the ring buffer (horizontal convolution and decimation)

//水平方向求解各個行和

for( ; sy <= y*2 + 2; sy++ )

{

//循環存儲行和

WT* row = buf + ((sy - sy0) % PD_SZ)*bufstep;

int _sy = borderInterpolate(sy, ssize.height, borderType);

const T* src = (const T*)(_src.data + _src.step*_sy);

int limit = cn;

const int* tab = tabL;

for( x = 0;;)

{

for( ; x < limit; x++ )

{

row[x] = src[tab[x+cn*2]]*6 + (src[tab[x+cn]] + src[tab[x+cn*3]])*4 +

src[tab[x]] + src[tab[x+cn*4]];

}

if( x == dsize.width )

break;

if( cn == 1 )

{

for( ; x < width0; x++ )

row[x] = src[x*2]*6 + (src[x*2 - 1] + src[x*2 + 1])*4 +

src[x*2 - 2] + src[x*2 + 2];

}

else if( cn == 3 )

{

for( ; x < width0; x += 3 )

{

const T* s = src + x*2;

WT t0 = s[0]*6 + (s[-3] + s[3])*4 + s[-6] + s[6];

WT t1 = s[1]*6 + (s[-2] + s[4])*4 + s[-5] + s[7];

WT t2 = s[2]*6 + (s[-1] + s[5])*4 + s[-4] + s[8];

row[x] = t0; row[x+1] = t1; row[x+2] = t2;

}

else if( cn == 4 )

{

for( ; x < width0; x += 4 )

{

const T* s = src + x*2;

WT t0 = s[0]*6 + (s[-4] + s[4])*4 + s[-8] + s[8];

WT t1 = s[1]*6 + (s[-3] + s[5])*4 + s[-7] + s[9];

row[x] = t0; row[x+1] = t1;

t0 = s[2]*6 + (s[-2] + s[6])*4 + s[-6] + s[10];

t1 = s[3]*6 + (s[-1] + s[7])*4 + s[-5] + s[11];

row[x+2] = t0; row[x+3] = t1;

}

else

{

for( ; x < width0; x++ )

{

int sx = tabM[x];

row[x] = src[sx]*6 + (src[sx - cn] + src[sx + cn])*4 +

src[sx - cn*2] + src[sx + cn*2];

}

limit = dsize.width;

tab = tabR - x;

}

// do vertical convolution and decimation and write the result to the destination image

//循環取得各個行和的指針

for( k = 0; k < PD_SZ; k++ )

rows[k] = buf + ((y*2 - PD_SZ/2 + k - sy0) % PD_SZ)*bufstep;

row0 = rows[0]; row1 = rows[1]; row2 = rows[2]; row3 = rows[3]; row4 = rows[4];

x = vecOp(rows, dst, (int)_dst.step, dsize.width);

//每一行結束的位置，把該行所有像素點的濾波結果求解出來

for( ; x < dsize.width; x++ )

dst[x] = castOp(row2[x]*6 + (row1[x] + row3[x])*4 + row0[x] + row4[x]);

}

圖像梯度圖的計算

圖像梯度的實現也要設計到窗口計算問題，所以和上面提到的方法有類似的地方，也是先計算行和，再計算列和。程序采用了Sharr算子進行梯度的計算，x方向的梯度圖算子是

31030003103 y方向的梯度算子是

30310010303

OpenCV詳細代碼如下，注意這里程序計算出的x和y方向的梯度值分別存放在了一個雙通道的目標圖像中，每個通道占用一個方向的梯度值。

86static void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst)

{

using namespace cv;

using cv::detail::deriv_type;

int rows = src.rows, cols = src.cols, cn = src.channels(), colsn = cols*cn, depth = src.depth();

CV_Assert(depth == CV_8U);

dst.create(rows, cols, CV_MAKETYPE(DataType::depth, cn*2));

#ifdef HAVE_TEGRA_OPTIMIZATION

if (tegra::calcSharrDeriv(src, dst))

return;

#endif

int x, y, delta = (int)alignSize((cols + 2)*cn, 16);

AutoBuffer _tempBuf(delta*2 + 64);

deriv_type *trow0 = alignPtr(_tempBuf + cn, 16), *trow1 = alignPtr(trow0 + delta, 16);

#if CV_SSE2

__m128i z = _mm_setzero_si128(), c3 = _mm_set1_epi16(3), c10 = _mm_set1_epi16(10);

#endif

for( y = 0; y < rows; y++ )

{

const uchar* srow0 = src.ptr(y > 0 ? y-1 : rows > 1 ? 1 : 0);

const uchar* srow1 = src.ptr(y);

const uchar* srow2 = src.ptr(y < rows-1 ? y+1 : rows > 1 ? rows-2 : 0);

deriv_type* drow = dst.ptr(y);

// do vertical convolution

x = 0;

#if CV_SSE2

for( ; x <= colsn - 8; x += 8 )

{

__m128i s0 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow0 + x)), z);

__m128i s1 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow1 + x)), z);

__m128i s2 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow2 + x)), z);

__m128i t0 = _mm_add_epi16(_mm_mullo_epi16(_mm_add_epi16(s0, s2), c3), _mm_mullo_epi16(s1, c10));

__m128i t1 = _mm_sub_epi16(s2, s0);

_mm_store_si128((__m128i*)(trow0 + x), t0);

_mm_store_si128((__m128i*)(trow1 + x), t1);

}

#endif

for( ; x < colsn; x++ )

{

int t0 = (srow0[x] + srow2[x])*3 + srow1[x]*10;

int t1 = srow2[x] - srow0[x];

trow0[x] = (deriv_type)t0;

trow1[x] = (deriv_type)t1;

}

// make border

int x0 = (cols > 1 ? 1 : 0)*cn, x1 = (cols > 1 ? cols-2 : 0)*cn;

for( int k = 0; k < cn; k++ )

{

trow0[-cn + k] = trow0[x0 + k]; trow0[colsn + k] = trow0[x1 + k];

trow1[-cn + k] = trow1[x0 + k]; trow1[colsn + k] = trow1[x1 + k];

}

// do horizontal convolution, interleave the results and store them to dst

x = 0;

#if CV_SSE2

for( ; x <= colsn - 8; x += 8 )

{

__m128i s0 = _mm_loadu_si128((const __m128i*)(trow0 + x - cn));

__m128i s1 = _mm_loadu_si128((const __m128i*)(trow0 + x + cn));

__m128i s2 = _mm_loadu_si128((const __m128i*)(trow1 + x - cn));

__m128i s3 = _mm_load_si128((const __m128i*)(trow1 + x));

__m128i s4 = _mm_loadu_si128((const __m128i*)(trow1 + x + cn));

__m128i t0 = _mm_sub_epi16(s1, s0);

__m128i t1 = _mm_add_epi16(_mm_mullo_epi16(_mm_add_epi16(s2, s4), c3), _mm_mullo_epi16(s3, c10));

__m128i t2 = _mm_unpacklo_epi16(t0, t1);

t0 = _mm_unpackhi_epi16(t0, t1);

// this can probably be replaced with aligned stores if we aligned dst properly.

_mm_storeu_si128((__m128i*)(drow + x*2), t2);

_mm_storeu_si128((__m128i*)(drow + x*2 + 8), t0);

}

#endif

for( ; x < colsn; x++ )

{

deriv_type t0 = (deriv_type)(trow0[x+cn] - trow0[x-cn]);

deriv_type t1 = (deriv_type)((trow1[x+cn] + trow1[x-cn])*3 + trow1[x]*10);

drow[x*2] = t0; drow[x*2+1] = t1;

}

標準LK算法的迭代

這個部分的實現需要注意的地方主要是subpixel的計算，因為每次計算出的位移都很小，考慮到計算的精度，必須的精確到小數位，所以需要注意如何計算一個小數位置的像素值，這個就和線性插值是類似的。如下圖所示，小數位置的像素值是有四個相鄰像素擬合出來的。設中間藍色像素點的坐標為(xsub,ysub)，四周四個整數位置的像素點自分別為(x0,y0),(x0,y1),(x1,y0),(x1,y1)，中間藍色像素離其他四個像素水平和垂直方向上的像素距離分別為w00,w01,w10,w11,如圖中所標。

subpxiel的計算

subpxiel的計算公式為

I(xsub,ysub)=w11w01I(x0,y0)+w10w01I(x0,y1)+w11w00I(x1,y0)+w10w00I(x1,y1)

在計算權重wi,j的時候，為了避免浮點計算，會對wi,j乘以一定的倍數使用整數運算。用的subpixel的地方主要有兩個地方，一個是在計算矩陣G的時候，需要取梯度圖的值，但是點的位置不一定是整數，所以一定要使用subpixel取值；還有一個地方是計算b的時候，因為要取前后兩幅圖像的像素值，而這兩個點的位置也不一定是整數，所以也要用到subpixel。考慮到這部分的OpenCV代碼比較長，而理解相對沒有那么困難，這里就不再貼出，僅列出上面需要注意的地方，需要的可以參考oepncv2.4.8/video/lkpramid.cpp/line:159-483

尾聲

LK算法的實現除了以上所講的OpenCV的實現外，還有幾個其他的版本，分別是由Stan Birchfield實現的版本KLT，速率相比OpenCV慢一些；一個GPU加速實現的版本GPU KLT；一個Matlab實現的版本Matlab KLT以及一個Java實現的版本Java KLT。

Bruce D. Lucas and Takeo Kanade. An Iterative Image Registration Technique with an Application to Stereo Vision. International Joint Conference on Artificial Intelligence, pages 674-679, 1981.

Carlo Tomasi and Takeo Kanade. Detection and Tracking of Point Features. Carnegie Mellon University Technical Report CMU-CS-91-132, April 1991.

Jianbo Shi and Carlo Tomasi. Good Features to Track. IEEE Conference on Computer Vision and Pattern Recognition, pages 593-600, 1994.

Jean-Yves Bouguet. Pyramidal Implementation of the Lucas Kanade Feature Tracker.