一、块匹配光流法基本原理：
将视频序列图像的每一帧分成很多互不重叠的块，并认为块内所有像素的位移矢量是一致的。然后，对于当前帧中的某块，根据一定的匹配准则，在参考帧的给定搜索范围内找出与此块最为相似的块，由相似度最高的块与当前块的相对位置计算出运动位移，所得到的运动位移即为当前块中每个像素点的运动矢量，当前帧中所有小块的运动矢量的集合为当前帧的光流场。
如图所示，假设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.输出光流。