基于LK光流算法的C语言实现

#include <string.h> #include <assert.h> #include <stdlib.h> #include <stdio.h> #include <math.h> #include "OpticalFlow.h" //#include <arm_neon.h> //#define _NEON_ //#define _NEON_ASM_ lk_inner *g_pLkInner[MAX_THREAD_NUM]={0}; #ifndef CHECK_RET_VAL #define CHECK_RET_VAL(chk, val) if ((chk)) {return (val);} #endif #define CHECK_FREE(addr) if((addr)) {free((addr)); (addr) = 0;} #define SIZE_ALIGN(SIZE, align) (((SIZE) + (align - 1)) & ((u32)(-align))) // align computation u8* alloc_buff(BUFFER_MGR *buff, s32 size) { u8 *pos = 0; s32 nAlign = 16; CHECK_RET_VAL(0 == buff, 0); CHECK_RET_VAL((buff->curr + SIZE_ALIGN(size, nAlign)) > buff->total, 0); pos = buff->start + buff->curr; buff->curr += SIZE_ALIGN(size, nAlign); memset(pos, 0, SIZE_ALIGN(size, nAlign)); return pos; } //申请金字塔各层图像空间及指针 void mvLKinit(int wid,int hgt,int nId) { int i = 0 ; int memSize,memMax; int sum; trajecy *pTrajecy = NULL; if(g_pLkInner[nId]) { return; } g_pLkInner[nId] = (lk_inner *) malloc(sizeof(lk_inner)); memSize = wid * hgt; memMax = K_IT * sizeof(bsdsize) + MAX_CORNERS * sizeof(bsdCorner) * 6 + (K_IT + 1) * memSize\ +sizeof(uint16) * MAX_CORNERS * 3 + sizeof(bsdCorner) * MAX_CORNERS + memSize + \ + MAX_TRAJECY_NUM * sizeof(trajecy) + MAX_TRAJECY_NUM * MAX_POINT_NUM_PER_TRAJECY * sizeof(bsdCorner) + \ + MAX_TRAJECY_NUM * sizeof(bsdCorner*) + 1024 * 4; //开辟内存 g_pLkInner[nId]->mem.start = (u8 *) malloc(memMax); memset(g_pLkInner[nId]->mem.start, 0, memMax); g_pLkInner[nId]->mem.curr = 0; g_pLkInner[nId]->mem.total = memMax; g_pLkInner[nId]->mem.end = g_pLkInner[nId]->mem.start + g_pLkInner[nId]->mem.total; //金字塔每层的长宽 g_pLkInner[nId]->SizeOfPyr=(bsdsize *)alloc_buff(&g_pLkInner[nId]->mem, K_IT * sizeof(bsdsize));//fang //g_pLkInner g_pLkInner[nId]->cornersA = (bsdCorner *)alloc_buff(&g_pLkInner[nId]->mem, MAX_CORNERS * sizeof(bsdCorner)); g_pLkInner[nId]->cornersB = (bsdCorner *)alloc_buff(&g_pLkInner[nId]->mem, MAX_CORNERS * sizeof(bsdCorner)); g_pLkInner[nId]->cornersC = (bsdCorner *)alloc_buff(&g_pLkInner[nId]->mem, MAX_CORNERS * sizeof(bsdCorner)); g_pLkInner[nId]->ctemp = (bsdCorner *)alloc_buff(&g_pLkInner[nId]->mem, MAX_CORNERS * sizeof(bsdCorner)); g_pLkInner[nId]->Matched_Precorners = (bsdCorner *)alloc_buff(&g_pLkInner[nId]->mem, MAX_CORNERS*sizeof(bsdCorner)); g_pLkInner[nId]->Matched_corners = (bsdCorner *)alloc_buff(&g_pLkInner[nId]->mem, MAX_CORNERS*sizeof(bsdCorner)); //set every floor size g_pLkInner[nId]->SizeOfPyr[0].x= wid; g_pLkInner[nId]->SizeOfPyr[0].y= hgt; for( i = 1; i < K_IT ; i++ )//fang { g_pLkInner[nId]->SizeOfPyr[i].x = g_pLkInner[nId]->SizeOfPyr[i-1].x >> 1; g_pLkInner[nId]->SizeOfPyr[i].y = g_pLkInner[nId]->SizeOfPyr[i-1].y >> 1; } //构建金子塔指针 //fang 此处修改比较大 拓展成 层数可任意设定 for( i = 0 ; i < K_IT ; i++) { //求图像大小 sum = g_pLkInner[nId]->SizeOfPyr[i].x * g_pLkInner[nId]->SizeOfPyr[i].y; g_pLkInner[nId]->const_thisPyr[i] = (UCHAR *)alloc_buff(&g_pLkInner[nId]->mem, sum * sizeof(UCHAR)); memset( g_pLkInner[nId]->const_thisPyr[i],0, sum*sizeof(UCHAR) ); g_pLkInner[nId]->const_prePyr[i] = (UCHAR *)alloc_buff(&g_pLkInner[nId]->mem, sum * sizeof(UCHAR) ); } g_pLkInner[nId]->features_found_init = (uint16 *)alloc_buff(&g_pLkInner[nId]->mem, sizeof(uint16) * MAX_CORNERS ); for (i = 0 ; i< MAX_CORNERS ;i++ ) { g_pLkInner[nId]->features_found_init[i] = 1; } g_pLkInner[nId]->features_found = (uint16 *)alloc_buff(&g_pLkInner[nId]->mem,sizeof(uint16) * MAX_CORNERS ); g_pLkInner[nId]->m_Index = (uint16 *)alloc_buff(&g_pLkInner[nId]->mem,sizeof(uint16) * MAX_CORNERS ); g_pLkInner[nId]->choose_point= (bsdCorner *)alloc_buff(&g_pLkInner[nId]->mem, sizeof(bsdCorner) * MAX_CORNERS ); g_pLkInner[nId]->pTrajecySet = (trajecy *)alloc_buff(&g_pLkInner[nId]->mem, MAX_TRAJECY_NUM * sizeof(trajecy)); for (i = 0 ; i < MAX_TRAJECY_NUM; i++) { pTrajecy = g_pLkInner[nId]->pTrajecySet + i; pTrajecy->point =(bsdCorner *)alloc_buff(&g_pLkInner[nId]->mem, MAX_POINT_NUM_PER_TRAJECY * sizeof(bsdCorner)); pTrajecy->ntrackId = TRAJECY_INVALID; pTrajecy->PoitNum = 0; } g_pLkInner[nId]->LastTrajCorn = (bsdCorner **)alloc_buff(&g_pLkInner[nId]->mem, MAX_TRAJECY_NUM * sizeof(bsdCorner*)); g_pLkInner[nId]->nTrajecyNum = 0 ; g_pLkInner[nId]->nGetPreImg = 0; g_pLkInner[nId]->corner_num = 0; g_pLkInner[nId]->matched_corner_num = 0; g_pLkInner[nId]->prePyr = &g_pLkInner[nId]->const_prePyr[0]; g_pLkInner[nId]->thisPyr= &g_pLkInner[nId]->const_thisPyr[0]; } /************************************************************************/ /* 计算上层金字塔 [in] lowerImg 下一层金字塔图像 UCHAR * [in] lowerImgSize size of lowerImg bsdsize * [in] upperImgSize size of upperImg bsdsize * [out] upperImg 上一层金字塔图像 UCHAR * */ /************************************************************************/ uint16 CalSubPyr(const UCHAR * lowerImg, UCHAR * upperImg, bsdsize *lowerImgSize,\ bsdsize *upperImgSize, int nYThresh, int nId) { int lowerImgWidth = lowerImgSize->x; int upperImgWidth = upperImgSize->x; unsigned short U16 = 0; unsigned short U8 = 0; unsigned short U4 = 0; // unsigned short U0 = 0; int i , j; int pos; nYThresh = (nYThresh >>1); for (j= nYThresh;j < upperImgSize->y-1;j++) { for(i=0;i<upperImgWidth-1;i++) { //8邻域 4个角 pos = (j * lowerImgWidth + i) << 1;//(x,y)<->(2x,2y) U16 = lowerImg[pos]+ lowerImg[pos + 2]+ lowerImg[pos + (lowerImgWidth << 1)]+ lowerImg[pos + (lowerImgWidth << 1) + 2]; U8 = lowerImg[pos + 1]+ lowerImg[pos + (lowerImgWidth << 1) + 1]+ lowerImg[pos + lowerImgWidth]+ lowerImg[pos + lowerImgWidth + 2]; U4 = lowerImg[pos + lowerImgWidth + 1]; *(upperImg+j*upperImgWidth+i)=(U16 + (U8 << 1) + (U4 << 2)) >> 4; } } return 1; } uint16 myCreatePyr(UCHAR**Pyr, int nYThresh, int nId) { int i; //逐层建立金字塔 for(i = 0;i < K_IT - 1; i++) { nYThresh = ( nYThresh >> i); CalSubPyr(Pyr[i], Pyr[i + 1], g_pLkInner[nId]->SizeOfPyr + i, g_pLkInner[nId]->SizeOfPyr + i + 1, nYThresh, nId); } return 1; } void oast9_16(const UCHAR* im, int xsize, int ysize, int b, \ int* num_corners,bsdCorner *pXyCorner,int nStart_y,int nEnd_y,int nStart_x,int nEnd_x, int nId) { int total = 0; /*int stepx =MAX((nEnd_x-nStart_x)/240,1); int stepy =MAX((nEnd_y-nStart_y)/80,1);*/ int stepx =3; int stepy =2; register int x, y; register int offset0, offset1, offset2, offset3, offset4, offset5, offset6, offset7, \ offset8, offset9, offset10, offset11, offset12, offset13, offset14, offset15; register int width; int cb ; int c_b; int start_y,End_y; start_y = nStart_y; End_y = nEnd_y; offset0=(-3)+(0)*xsize; offset1=(-3)+(-1)*xsize; offset2=(-2)+(-2)*xsize; offset3=(-1)+(-3)*xsize; offset4=(0)+(-3)*xsize; offset5=(1)+(-3)*xsize; offset6=(2)+(-2)*xsize; offset7=(3)+(-1)*xsize; offset8=(3)+(0)*xsize; offset9=(3)+(1)*xsize; offset10=(2)+(2)*xsize; offse