PX4FLOW光流模块DIY（含部分代码讲解）
暑假有时间整理一下以前做的东西，发发博客，既给网友们学习也方便自己交流。今天讲讲我两年前从github学习的PX4FLOW光流模块。
光流是视觉导航的重要部分。在运动检测和许多slam技术都使用到了光流，但是大部分还是处于理论阶段，在实际运用到的地方不多，我所知道的一个是电脑的鼠标使用到了光流，还有一个就是在飞行器的室内导航(因为室内基本接收不到GPS导航信号况且精度不够)。想知道光流原理的可以去百度必应找找，如果看不了那里的讲解可以试着理解我下面这段话，如果都理解不了劝你不要继续往下看。
我说说光流的基本原理：在连续捕获的图片流数据中找到当前帧图片中的具备某些特征的点，称其为特征点（特征点是比较好标定的一堆像素块：可以是3X3,4X4,8X8,9X9的像素块，一般都是取角点），标定其所在当前帧的像素的坐标（x1,y1），然后在下一帧图片中找到同一个特征点，记录像素坐标(x2,y2)，两帧图片之间的时间间隔是Δt。那么在这段时间间隔内的速度就是：x方向上：（x2-x1）/Δt，y方向上：（y2-y1）/Δt。
下面讲解PX4FLOW：
既然是制作实物首先还是需要制作一个实验用的硬件平台，如下是其使用到的一些芯片
主控MCU:STM32F407
传感器有摄像头：MT9V034,陀螺仪：L3G4200D,sonar声呐模块。
为了试验需要我使用了LCD显示图片，个人只想做视觉部分所以把陀螺仪和声呐部分都去除了下面是我设计的原理图
设计并制作好的PCB电路板：
下面开始编写代码，我只用了PX4FLOW的光流算法的代码，其余的摄像头驱动LCD驱动都是我自己参考各种手册写的。
Systick_Init();
Delay_us(1000);
USART1_Config(57600);
printf("USART1 Configure OK!\n");
Delay_us(50);
/* enable FPU on Cortex-M4F core */
SCB->CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2));
LCD_Init();
LED_Init();
MT9V034_Init();
DCMI_Configure();
DCMI_Start();
主函数中主要的初始化代码。
LCD和LED初始化我就不在这里累述了。野火原子都已经讲得很详细了。这里我将一下DCMI这个驱动，这是stm32f407自带的一个捕获并行像素数据的外设端口。同样我们使用库函数的话只用对结构体进行填充。下面是我的代码。
void DCMI_Configure(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
DCMI_InitTypeDef DCMI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
//step 1 :Enable the clock for the DCMI and associated GPIOs
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB|RCC_AHB1Periph_GPIOC|RCC_AHB1Periph_GPIOE, ENABLE);
RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_DCMI,ENABLE);
//step 2 :DCMI pins configuration
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4|GPIO_Pin_6;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOA,&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7|GPIO_Pin_6|GPIO_Pin_8|GPIO_Pin_9;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOB,&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8/*|GPIO_Pin_9*/|GPIO_Pin_11;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOC,&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOE,&GPIO_InitStructure);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource4, GPIO_AF_DCMI);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource6, GPIO_AF_DCMI);
GPIO_PinAFConfig(GPIOB, GPIO_PinSource6, GPIO_AF_DCMI);
GPIO_PinAFConfig(GPIOB, GPIO_PinSource7, GPIO_AF_DCMI);
GPIO_PinAFConfig(GPIOB, GPIO_PinSource8, GPIO_AF_DCMI);
GPIO_PinAFConfig(GPIOB, GPIO_PinSource9, GPIO_AF_DCMI);
GPIO_PinAFConfig(GPIOC, GPIO_PinSource6, GPIO_AF_DCMI);
GPIO_PinAFConfig(GPIOC, GPIO_PinSource7, GPIO_AF_DCMI);
GPIO_PinAFConfig(GPIOC, GPIO_PinSource8, GPIO_AF_DCMI);
//GPIO_PinAFConfig(GPIOC, GPIO_PinSource9, GPIO_AF_DCMI);
//GPIO_PinAFConfig(GPIOE, GPIO_PinSource0, GPIO_AF_DCMI); // D2
GPIO_PinAFConfig(GPIOE, GPIO_PinSource1, GPIO_AF_DCMI); // D3
//GPIO_PinAFConfig(GPIOE, GPIO_PinSource4, GPIO_AF_DCMI); // D4
//GPIO_PinAFConfig(GPIOE, GPIO_PinSource5, GPIO_AF_DCMI); // D6
//GPIO_PinAFConfig(GPIOE, GPIO_PinSource6, GPIO_AF_DCMI); // D7
GPIO_PinAFConfig(GPIOC, GPIO_PinSource11,GPIO_AF_DCMI);
DCMI_DeInit();//Çå³ýÔÀ´µÄÉèÖÃ
//step 3 :Declare a DCMI_InitTypeDef structure
DCMI_InitStructure.DCMI_CaptureMode = DCMI_CaptureMode_Continuous;//The received data are transferred continuously
DCMI_InitStructure.DCMI_SynchroMode = DCMI_SynchroMode_Hardware;//ÉèÖÃΪӲ¼þͬ²½
DCMI_InitStructure.DCMI_PCKPolarity = DCMI_PCKPolarity_Rising; //ÉèÖÃÏñËصÄʱÖÓÔÚÉÏÉýÑؼ¤»î
DCMI_InitStructure.DCMI_VSPolarity = DCMI_VSPolarity_Low; //Ö¸¶¨´¹Ö±Í¬²½ÐźÅÊÇÔڸߵçƽµÄʱºò¹¤×÷
DCMI_InitStructure.DCMI_HSPolarity = DCMI_HSPolarity_Low; //Ö¸¶¨Ë®Æ½Í¬²½ÐźÅÊÇÔڸߵçƽµÄʱºò¹¤×÷
DCMI_InitStructure.DCMI_CaptureRate = DCMI_CaptureRate_All_Frame;
DCMI_InitStructure.DCMI_ExtendedDataMode = DCMI_ExtendedDataMode_8b;
//step 4 :Initialize the DCMI interface
DCMI_Init(&DCMI_InitStructure);
DCMI_Cmd(ENABLE);
NVIC_InitStructure.NVIC_IRQChannel = DCMI_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
DCMI_ITConfig(DCMI_IT_FRAME,ENABLE);
//step 5 :Configure the DMA2_Stream1 channel1 to transfer Data from DCMI DR register to the destination memory buffer
DCMI_DMA_Init((u32)image,4096,DMA_MemoryDataSize_Byte,DMA_MemoryInc_Enable);
}
使用中文注释的都是乱码，英文注释都在。
摄像头模块如果没有自带晶振的话需要控制芯片给它提供方波。STM32F4正好有两个引脚是可以配置成对外输送方波的，他们是MCO1和MCO2,配置方法可以去参考意法半导体公司给的官方数据手册。把输出脉冲配置成MT9V034芯片所需要的脉冲频率范围内（小于27MHz，建议配置成26MHz,stm32提供的脉冲很不稳定）的方波。对于摄像头芯片的初始化，可以参考MT9V034的数据手册，其通信协议跟SCCB极端的类似（SCCB和I2C通信又是非常相似的），但是需要改变的一个是它的通信数据传输长度是16位数据，这个做相应的修改就好的修改非常简单，根据数据手册修改就好了。下面是对摄像头芯片修改的一些代码。
SCCB_WR_16Reg(MTV_CHIP_CONTROL_REG,0X8188);//904
//Context A
SCCB_WR_16Reg(MTV_WINDOW_WIDTH_REG_A,256);//752
SCCB_WR_16Reg(MTV_WINDOW_HEIGHT_REG_A,256);//480
SCCB_WR_16Reg(MTV_HOR_BLANKING_REG_A,587);//94
SCCB_WR_16Reg(MTV_VER_BLANKING_REG_A,10);//45
SCCB_WR_16Reg(MTV_READ_MODE_REG_A,0X30A);//768
SCCB_WR_16Reg(MTV_COLUMN_START_REG_A,249);//1
SCCB_WR_16Reg(MTV_ROW_START_REG_A,116);//4
SCCB_WR_16Reg(MTV_COARSE_SW_1_REG_A,0x1bb);//443
SCCB_WR_16Reg(MTV_COARSE_SW_2_REG_A,0X01D9);//473
SCCB_WR_16Reg(MTV_COARSE_SW_CTRL_REG_A,0X0164);//356
SCCB_WR_16Reg(MTV_V2_CTRL_REG_A,0X01E0);
//General Setting
SCCB_WR_16Reg(MTV_ROW_NOISE_CORR_CTRL_REG,0X0000);
SCCB_WR_16Reg(MTV_AEC_AGC_ENABLE_REG,0X0303);
//recommed register settings
SCCB_WR_16Reg(MTV_MIN_EXPOSURE_REG,0X0001);
SCCB_WR_16Reg(MTV_MAX_EXPOSURE_REG,0X0030);//0x0030
SCCB_WR_16Reg(MTV_MAX_GAIN_REG,64);
SCCB_WR_16Reg(MTV_AGC_AEC_PIXEL_COUNT_REG,4096);
SCCB_WR_16Reg(MTV_AGC_AEC_DESIRED_BIN_REG,30);
SCCB_WR_16Reg(MTV_ADC_RES_CTRL_REG,0X0303);
//Reset
SCCB_WR_16Reg(MTV_SOFT_RESET_REG,0X01);
LCD驱动也写好了实验一下显示图片。
如此便可以做一些实验了。从网络上获得快速角点检测的方法编一个代码试试。下面是一些结果图片，效果还是很不错的。就是有点慢。
下面我把PX4FLOW光流的代码贴出来。
uint16_t compute_flow(uint8_t image1,uint8_t *image2/,float x_rate,float y_rate,float pixel_flow_x,float *pixel_flow_y/)
{
const signed short int winmin = -4;
const signed short int winmax = 4;
const uint16_t hist_size = 2*(winmax-winmax)+1;
float meanflowx = 0.0f;
float meanflowy = 0.0f;
uint16_t meancount = 0;
uint8_t subdirs[64];
uint16_t i,j;
uint32_t acc[8];
uint16_t histx[hist_size];
uint16_t histy[hist_size];
for(j=0;j6)
{
meanflowx /= meancount;
meanflowy /= meancount;
histflowx += meanflowx;
flow_y += meanflowy;
LCD_Rectangle(100,200,30,30,WHITE);
LCD_Str_6x12_O(60,200,"Y flow:",BLACK);
LCD_Num_6x12_O(100,200,100+histflowx/scale,BLACK);
LCD_Str_6x12_O(60,220,"X flow:",BLACK);
LCD_Num_6x12_O(100,220,100-flow_y/scale,BLACK);
LCD_Rectangle(80-(flow_y/scale),160+histflowx/scale,1,1,RED);
/*
if(histx[maxpositionx]>meancount/6 && histy[maxpositiony]>meancount/6)
{
// const float focal_length_px = LENGTH/(4.0f*6.0f)*1000.0f;
if(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;
float hist_x_value = 0.0f;
float hist_x_weight = 0.0f;
float hist_y_value = 0.0f;
float hist_y_weight = 0.0f;
uint8_t i1;
if(maxpositionx>1 && maxpositionx1 && maxpositiony }
他们计算角点使用的并不是什么特征点检测算法，而是直接使用了梯度计算方法。在一个4X4的像素块中灰度像素的变化比较明显就认为是比较好的点（特征点），下面是代码：
uint32_t compute_diff(uint8_t *image,uint16_t offx,uint16_t offy,uint16_t row_size)
{
uint32_t acc;
uint16_t off = (offy+2)*row_size+offx;
uint32_t col1=(image[off+0+0*row_size]<<24)|(image[off+0+1*row_size]<<16)|(image[off+0+2*row_size]<<8)|(image[off+0+3*row_size]);
uint32_t col2=(image[off+1+0*row_size]<<24)|(image[off+1+1*row_size]<<16)|(image[off+1+2*row_size]<<8)|(image[off+1+3*row_size]);
uint32_t col3=(image[off+2+0*row_size]<<24)|(image[off+2+1*row_size]<<16)|(image[off+2+2*row_size]<<8)|(image[off+2+3*row_size]);
uint32_t col4=(image[off+3+0*row_size]<<24)|(image[off+3+1*row_size]<<16)|(image[off+3+2*row_size]<<8)|(image[off+3+3*row_size]);
acc = __USAD8(*((uint32_t*)&image[off+0+0*row_size]),*((uint32_t*)&image[off+0+1*row_size]));
acc = __USADA8(*((uint32_t*)&image[off+0+1*row_size]),*((uint32_t*)&image[off+0+2*row_size]),acc);
acc = __USADA8(*((uint32_t*)&image[off+0+2*row_size]),*((uint32_t*)&image[off+0+3*row_size]),acc);
acc = __USADA8(col1,col2,acc);
acc = __USADA8(col2,col3,acc);
acc = __USADA8(col3,col4,acc);
return acc;
}
代码有使用到simd指令，simd指令是嵌入在单片机里面比较高效的一些指令，主要是用于处理一些重复率比较大，数据比较长的运算。理论上讲是能把运算效率提高75%。下面我就讲一个函数
__USAD8(),这个函数的意思是unsigned sum of absolute difference。两组四个8位数的数据分别做差然后求出四个差的绝对值之和返回一个无符号的32位数据。有此基础DIY光流模块就方便了。
下面是我移植在MDK上的工程代码的截图
下面是我实际测量的数据显示
我走的是一个10mX10m的方块，模块距离地面高度1m，有累计的方法在LCD屏上打点，受累计误差的影响首位不重合。精度还是可以的。中间显示的是摄像头捕获到的图像的一部分。
学习别人的代码重在理解其中的算法和程序设计的框架。学习就要抱着学习的态度，不是天天在技术群里吹牛逼，做伸手党可以的。
--山东大学机器人研究中心某硕士