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;j<hist_size;j++){histx[j]=0;histy[j]=0;}

for(j=17;j<64-23;j+=3)
{
    for(i=17;i<64-23;i+=3)
    {
        uint32_t diff = compute_diff(image1,i,j,64);//uint32_t diff = compute_diff(image1,i,j,64);
        uint32_t dist = 0xffffffff;
        int8_t sumx = 0;
        int8_t sumy = 0;
        int8_t ii,jj;
        if(diff<diff_val){continue;}//30
        for(jj=winmin;jj<=winmax;jj++)
        {
            for(ii=winmin;ii<=winmax;ii++)
            {
                uint32_t temp_dist = compute_sad_8x8(image1,image2,i,j,i+ii,j+jj,IMAGE_WIDTH);
                if(temp_dist<dist)
                {
                    sumx=ii;
                    sumy=jj;
                    dist=temp_dist;
                }
            }
        }


        if(dist<FLOW_THRESHOLD)//100 
        {
            uint32_t mindist = dist;
            uint8_t mindir = 8;
            int8_t i3;
            uint8_t hist_index_x = 2*sumx + (winmax-winmin+1);
            uint8_t hist_index_y = 2*sumy + (winmax-winmin+1);

            meanflowx += (float)sumx;
            meanflowy += (float)sumy;

            compute_subpixel(image1,image2,i,j,i+sumx,j+sumy,acc,(uint16_t)IMAGE_WIDTH);
            for(i3=0;i3<8;i3++)
            {
                if(acc[i3]<mindist)
                {
                    mindist = acc[i3];
                    mindir = i3;
                }
            }
            subdirs[meancount]=mindir;
            meancount++;

                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;
                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]++;
        }
    }
}
    if(meancount>6)
    {


        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 && 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;
                }
                for(i1=hist_x_min;i1<hist_x_max+1;i1++)
                {
                    hist_x_value += (float)(i1*histx[i1]);
                    hist_x_weight += (float)histx[i];
                }
                for(i=hist_y_min;i<hist_y_max+1;i++)
                {
                    hist_y_value += (float)(i*histy[i]);
                    hist_y_weight += (float)histy[i];
                }
                histflowx = (hist_x_value/hist_x_weight - (winmax-winmin+1));
                histflowy = (hist_y_value/hist_y_weight - (winmax-winmin+1));
            }
            else
            {
                uint32_t meancount_x = 0;
                uint32_t meancount_y = 0;
                uint8_t i2;
                for(i2=0;i2<meancount;i2++)
                {
                    float subdirx = 0.0f;
                    float subdiry = 0.0f;
                    if(subdirs[i2]==0||subdirs[i2]==1||subdirs[i2]==7) subdirx = 0.5f;
                    if(subdirs[i2]==3||subdirs[i2]==3||subdirs[i2]==5) subdirx = -0.5f;
                    histflowx += (float)dirsx[i]+subdirx;
                    meancount_x++;

                    if(subdirs[i2]==5||subdirs[i2]==6||subdirs[i2]==7) subdiry = 0.5f;
                    if(subdirs[i2]==1||subdirs[i2]==2||subdirs[i2]==3) subdiry = -0.5f;
                    histflowy += (float)dirsy[i]+subdiry;
                    meancount_y++;
                }
                histflowx /= meancount_x;
                histflowy /= meancount_y;
            }
        }*/
    }

return meancount;

}
他們計算角點使用的並不是什麼特徵點檢測算法，而是直接使用了梯度計算方法。在一個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屏上打點，受累計誤差的影響首位不重合。精度還是可以的。中間顯示的是攝像頭捕獲到的圖像的一部分。
學習別人的代碼重在理解其中的算法和程序設計的框架。學習就要抱着學習的態度，不是天天在技術羣裏吹牛逼，做伸手黨可以的。

                                       --山東大學機器人研究中心某碩士

PX4FLOW光流模塊DIY（含部分代碼講解）

知識普及篇——動手做foc無刷電機電子調速器

你想不到的程序員的技能

磁力計校正和數據處理

cleanflight成功移植到MDK(keil)

DIY四旋翼（穿越機）軟硬件設計

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結