一、塊比對光流法基本原理：
将視訊序列圖像的每一幀分成很多互不重疊的塊，并認為塊内所有像素的位移矢量是一緻的。然後，對于目前幀中的某塊，根據一定的比對準則，在參考幀的給定搜尋範圍内找出與此塊最為相似的塊，由相似度最高的塊與目前塊的相對位置計算出運動位移，所得到的運動位移即為目前塊中每個像素點的運動矢量，目前幀中所有小塊的運動矢量的集合為目前幀的光流場。
如圖所示，假設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方向的光流總值。
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]))
{
先看直方圖法：
首先找到直方圖的峰值：
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]))
{
uint16_t hist_x_min = maxpositionx;
uint16_t hist_x_max = maxpositionx;
uint16_t hist_y_min = maxpositiony;
uint16_t hist_y_max = maxpositiony;
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;
}
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
{
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])
{
float y_rate_pixel = y_rate * (get_time_between_images() / 1000000.0f) * focal_length_px;
float comp_x = histflowx + y_rate_pixel;
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])
{
float x_rate_pixel = x_rate * (get_time_between_images() / 1000000.0f) * focal_length_px;
float comp_y = histflowy - x_rate_pixel;
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;
}
四、與超音波結合
最後和超音波結合得到最終地面的速度：
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);
if (distance_valid)
{
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.輸出光流。