一、塊匹配光流法基本原理：
將視頻序列圖像的每一幀分成很多互不重疊的塊，並認爲塊內所有像素的位移矢量是一致的。然後，對於當前幀中的某塊，根據一定的匹配準則，在參考幀的給定搜索範圍內找出與此塊最爲相似的塊，由相似度最高的塊與當前塊的相對位置計算出運動位移，所得到的運動位移即爲當前塊中每個像素點的運動矢量，當前幀中所有小塊的運動矢量的集合爲當前幀的光流場。
如圖所示，假設t時刻對應當前幀G0，在t`時刻對應參考幀G1，將圖像G0分割成許多互不重疊且大小爲M*N的小塊，在給定搜索步長（um,vm）後，對所有小塊進行匹配運算，對於其中中心點座標爲（x,y）的小塊，在G1中對應的小塊的起始搜索座標範圍爲從（x-um,y-vm）到（x+um,y+vm）的區域，將搜索區域內的所有小塊與G0中的小塊進行匹配運算，得到的誤差最小塊，即爲最佳匹配塊。最佳匹配塊的起始點座標與G0中小塊的起始點座標（x,y）之差即爲G0中此小塊內所有像素點的運動矢量。
在實際應用中，爲了計算方便，往往取小塊的M,N值爲相等且值爲2的
倍數，步長um,vm也取值相等。塊匹配光流算法的精度收到塊大小M,N值的影響，當M,N值過大時，塊內所有像素的位移矢量一致的假設條件容易被破壞，當M,N值過小時，塊內信息量的減少會降低匹配塊的準確度。
二、代碼分析
一、確定匹配塊及對應光流：
for (j = pixLo; j < pixHi; j += pixStep)
{
for (i = pixLo; i < pixHi; i += pixStep)
{
這裏的for循環是遍歷當前幀的塊。
而在每一塊K中，我們要做如下操作:
（1）尋找角點，看該塊是否有必要進行匹配，若沒有角點，跳出此層循環，移動到下一個塊K+1；若有角點，進入下面的for循環
(2)尋找最佳匹配塊
for (jj = winmin; jj <= winmax; jj++)
{
for (ii = winmin; ii <= winmax; ii++)
{
這一步，是在參考幀中，（i,j）點的鄰域（winmin，winmax）範圍內，尋找與塊K匹配的塊K’。
根據如下公式，選擇連續兩幀圖片的最佳匹配塊
其中f(I,j)是灰度值，(i，j)是像素點的座標，(m,n)是位移。
具體代碼如下：
uint32_t temp_dist = ABSDIFF(base1, base2 + ii);
其中：*base1 = image1 + j * (uint16_t) global_data.param[PARAM_IMAGE_WIDTH] + i;
*base2 = image2 + (j+jj) * (uint16_t) global_data.param[PARAM_IMAGE_WIDTH] + i;
遍歷該鄰域範圍內的每一個像素，直到找到最相似的那個塊。
記錄其位移（ii，jj），相似度dist。
這裏比較的是以（i,j）(i+ii,j+jj)爲基點的左下方8*8的像素塊。
接下來，當上述相似度dist滿足一定的閾值範圍時，我們纔會進行如下操作：
if (dist < global_data.param[PARAM_BOTTOM_FLOW_VALUE_THRESHOLD])
{
meanflowx += (float) sumx;
meanflowy += (float) sumy;
compute_subpixel(image1, image2, i, j, i + sumx, j + sumy, acc, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]);
uint32_t mindist = dist; // best SAD until now
uint8_t mindir = 8; // direction 8 for no direction
for(uint8_t k = 0; k < 8; k++)
{
if (acc[k] < mindist)
{
// SAD becomes better in direction k
mindist = acc[k];
mindir = k;
}
}
這裏解釋一下compute_subpixel() 這個函數:
對於image2中的（i+ii,j+jj）這個位置的像素X，我們利用其周圍的8個像素0~7，計算出S0,S1,S2……S7,它們的位置如下
然後再利用S0,S1……S7,計算出T1,T3,T5,T7。S0,T1,S2,T3,S4,T5,S6,T7這8個值得位置如下
得到X周圍8個方向的子像素之後，我們再用這8個子像素分別與image1的（i，j）位置的像素做差比較，並存入acc[]。
對image1的第1列和第5列的每一個像素都做上述操作。如此便積累了這8個方向的dist和。
對於下面的for循環，目的是找到最小的acc[]。
爲什麼要找這個最小值呢？
光流不是回到原位，而是抑制其偏移運動。
我已經找到最相似的塊了（ii，jj），也就是知道物體移動到哪個位置了，我想找到一個與原圖像塊最匹配的方向，以便後續光流的彌補運動。
最後的一小段代碼，是直方圖統計，統計x,y方向的光流總值。
/* feed histogram filter*/
uint8_t hist_index_x = 2*sumx + (winmax-winmin+1);
if (subdirs[i] == 0 || subdirs[i] == 1 || subdirs[i] == 7) hist_index_x += 1;
if (subdirs[i] == 3 || subdirs[i] == 4 || subdirs[i] == 5) hist_index_x += -1;
uint8_t hist_index_y = 2*sumy + (winmax-winmin+1);
if (subdirs[i] == 5 || subdirs[i] == 6 || subdirs[i] == 7) hist_index_y += -1;
if (subdirs[i] == 1 || subdirs[i] == 2 || subdirs[i] == 3) hist_index_y += 1;
histx[hist_index_x]++;
histy[hist_index_y]++;
X方向
Y方向
二、求整體平均光流
所有塊的光流（位移）都找到之後，我們取平均，得到整體的運動情況，以便做補償。
這裏分兩種情況：
如果滿足如下條件，我們用直方圖均衡法來求光流的平均值，
否則，我們直接用位移累加值來求平均光流。
if (FLOAT_AS_BOOL(global_data.param[PARAM_BOTTOM_FLOW_HIST_FILTER]))
{
先看直方圖法：
首先找到直方圖的峯值：
/* position of maximal histogram peek */
for (j = 0; j < hist_size; j++)
{
if (histx[j] > maxvaluex)
{
maxvaluex = histx[j];
maxpositionx = j;
}
if (histy[j] > maxvaluey)
{
maxvaluey = histy[j];
maxpositiony = j;
}
}
確定直方圖峯值的一個小鄰域，並在該鄰域內取加權平均值作爲平均光流。
if (FLOAT_AS_BOOL(global_data.param[PARAM_BOTTOM_FLOW_HIST_FILTER]))
{
/* use histogram filter peek value */
uint16_t hist_x_min = maxpositionx;
uint16_t hist_x_max = maxpositionx;
uint16_t hist_y_min = maxpositiony;
uint16_t hist_y_max = maxpositiony;
/* x direction */
if (maxpositionx > 1 && maxpositionx < hist_size-2)
{
hist_x_min = maxpositionx - 2;
hist_x_max = maxpositionx + 2;
}
else if (maxpositionx == 0)
{
hist_x_min = maxpositionx;
hist_x_max = maxpositionx + 2;
}
else if (maxpositionx == hist_size-1)
{
hist_x_min = maxpositionx - 2;
hist_x_max = maxpositionx;
}
else if (maxpositionx == 1)
{
hist_x_min = maxpositionx - 1;
hist_x_max = maxpositionx + 2;
}
else if (maxpositionx == hist_size-2)
{
hist_x_min = maxpositionx - 2;
hist_x_max = maxpositionx + 1;
}
/* y direction */
if (maxpositiony > 1 && maxpositiony < hist_size-2)
{
hist_y_min = maxpositiony - 2;
hist_y_max = maxpositiony + 2;
}
else if (maxpositiony == 0)
{
hist_y_min = maxpositiony;
hist_y_max = maxpositiony + 2;
}
else if (maxpositiony == hist_size-1)
{
hist_y_min = maxpositiony - 2;
hist_y_max = maxpositiony;
}
else if (maxpositiony == 1)
{
hist_y_min = maxpositiony - 1;
hist_y_max = maxpositiony + 2;
}
else if (maxpositiony == hist_size-2)
{
hist_y_min = maxpositiony - 2;
hist_y_max = maxpositiony + 1;
}
float hist_x_value = 0.0f;
float hist_x_weight = 0.0f;
float hist_y_value = 0.0f;
float hist_y_weight = 0.0f;
for (uint8_t h = hist_x_min; h < hist_x_max+1; h++)
{
hist_x_value += (float) (h*histx[h]);
hist_x_weight += (float) histx[h];
}
for (uint8_t h = hist_y_min; h<hist_y_max+1; h++)
{
hist_y_value += (float) (h*histy[h]);
hist_y_weight += (float) histy[h];
}
histflowx = (hist_x_value/hist_x_weight - (winmax-winmin+1)) / 2.0f ;
histflowy = (hist_y_value/hist_y_weight - (winmax-winmin+1)) / 2.0f;
}
第二種情況，直接用位移累加值求平均。
else
{
/* use average of accepted flow values */
uint32_t meancount_x = 0;
uint32_t meancount_y = 0;
for (uint8_t h = 0; h < meancount; h++)
{
float subdirx = 0.0f;
if (subdirs[h] == 0 || subdirs[h] == 1 || subdirs[h] == 7) subdirx = 0.5f;
if (subdirs[h] == 3 || subdirs[h] == 4 || subdirs[h] == 5) subdirx = -0.5f;
histflowx += (float)dirsx[h] + subdirx;
meancount_x++;
float subdiry = 0.0f;
if (subdirs[h] == 5 || subdirs[h] == 6 || subdirs[h] == 7) subdiry = -0.5f;
if (subdirs[h] == 1 || subdirs[h] == 2 || subdirs[h] == 3) subdiry = 0.5f;
histflowy += (float)dirsy[h] + subdiry;
meancount_y++;
}
histflowx /= meancount_x;
histflowy /= meancount_y;
}
最終輸出結果histflowx，histflowy給旋轉補償。
三、旋轉補償
爲了減去由於攝像頭自身旋轉產生的光流，我們需要對運動場的旋轉部件進行旋轉補償，分別得到圖像平面x,y方向的光流分量vx，vy：
焦距f，和攝像頭角速度w是已知的。
這邊，飛行器的座標軸和光流的座標軸有如圖所示的關係，所以在補償運算時有如下公式：
* -y_rate gives x flow
* x_rates gives y_flow
當陀螺儀的轉動較大，大於閾值時，我們才做補償，如果微小偏移，可以忽略，不做補償。
並且，the compensated value is clamped to the maximum measurable flow value (param BFLOW_MAX_PIX) +0.5 (sub pixel flow can add half pixel to the value)
if(fabsf(y_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD])
{
/* calc pixel of gyro */
float y_rate_pixel = y_rate * (get_time_between_images() / 1000000.0f) * focal_length_px;
float comp_x = histflowx + y_rate_pixel;
/* clamp value to maximum search window size plus half pixel from subpixel search */
if (comp_x < (-SEARCH_SIZE - 0.5f))
*pixel_flow_x = (-SEARCH_SIZE - 0.5f);
else if (comp_x > (SEARCH_SIZE + 0.5f))
*pixel_flow_x = (SEARCH_SIZE + 0.5f);
else
*pixel_flow_x = comp_x;
}
else
{
*pixel_flow_x = histflowx;
}
if(fabsf(x_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD])
{
/* calc pixel of gyro */
float x_rate_pixel = x_rate * (get_time_between_images() / 1000000.0f) * focal_length_px;
float comp_y = histflowy - x_rate_pixel;
/* clamp value to maximum search window size plus/minus half pixel from subpixel search */
if (comp_y < (-SEARCH_SIZE - 0.5f))
*pixel_flow_y = (-SEARCH_SIZE - 0.5f);
else if (comp_y > (SEARCH_SIZE + 0.5f))
*pixel_flow_y = (SEARCH_SIZE + 0.5f);
else
*pixel_flow_y = comp_y;
}
else
{
*pixel_flow_y = histflowy;
}
四、與超聲波結合
最後和超聲波結合得到最終地面的速度：
/*
* real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z
* x / f = X / Z
* y / f = Y / Z
*/
float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000000.0f);
float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000000.0f);
/* integrate velocity and output values only if distance is valid */
if (distance_valid)
{
/* calc velocity (negative of flow values scaled with distance) */
float new_velocity_x = - flow_compx * sonar_distance_filtered;
float new_velocity_y = - flow_compy * sonar_distance_filtered;
1.2個for循環是尋找角點
2.後2個for是在角點的鄰域尋找匹配塊，鄰域是以角點爲中心的九宮格，尋找方法是以角點爲左上角畫8*8的塊 和 鄰域的每個像素爲左上角畫8*8的塊 來匹配的
3.統計方向
4.旋轉的補償
5.輸出光流。