#include"string.h" #include"assert.h" #include"stdlib.h" #include"math.h" #include"stdio.h" //#include <iostream> //#include <fstream> #define CV_PI 3.1415926535897932384626433832795 #define IPL_DEPTH_SIGN 0x80000000 #define IPL_DEPTH_1U 1 #define IPL_DEPTH_8U 8 #define IPL_DEPTH_16U 16 #define IPL_DEPTH_32F 32 #define IPL_DEPTH_8S (IPL_DEPTH_SIGN| 8) #define IPL_DEPTH_16S (IPL_DEPTH_SIGN|16) #define IPL_DEPTH_32S (IPL_DEPTH_SIGN|32) #define IPL_DATA_ORDER_PIXEL 0 #define IPL_DATA_ORDER_PLANE 1 #define IPL_ORIGIN_TL 0 #define IPL_ORIGIN_BL 1 #define IPL_ALIGN_4BYTES 4 #define IPL_ALIGN_8BYTES 8 #define IPL_ALIGN_16BYTES 16 #define IPL_ALIGN_32BYTES 32 #define IPL_ALIGN_DWORD IPL_ALIGN_4BYTES #define IPL_ALIGN_QWORD IPL_ALIGN_8BYTES #define IPL_BORDER_CONSTANT 0 #define IPL_BORDER_REPLICATE 1 #define IPL_BORDER_REFLECT 2 #define IPL_BORDER_WRAP 3 #define IPL_BORDER_REFLECT_101 4 #define IPL_BORDER_TRANSPARENT 5 #define DBL_EPSILON 2.2204460492503131e-016 /* smallest such that 1.0+DBL_EPSILON != 1.0 */ static const float atan2_p1 = 0.9997878412794807f*(float)(180/CV_PI); static const float atan2_p3 = -0.3258083974640975f*(float)(180/CV_PI); static const float atan2_p5 = 0.1555786518463281f*(float)(180/CV_PI); static const float atan2_p7 = -0.04432655554792128f*(float)(180/CV_PI); //typedef unsigned int size_t ; #define min2(a,b) ((a)<(b)?(a):(b)) typedef unsigned char uchar; const int nbins = 9; typedef struct myPoint { int x; int y; }myPoint; typedef struct mySize { int width; int height; }mySize; typedef struct BlockData { int histOfs; myPoint imgOffset; }BlockData; typedef struct PixData { size_t gradOfs, qangleOfs; int histOfs[4]; float histWeights[4]; float gradWeight; }PixData; typedef struct myHOGDescriptor { mySize winSize; mySize blockSize; mySize blockStride; mySize cellSize; int nbins; double winSigma; double L2HysThreshold; }myHOGDescriptor; typedef struct myHOGCache { mySize winSize, cacheStride; mySize nblocks, ncells; int blockHistogramSize; int count1, count2, count4; //vector<pixdata> pixData; //vector<blockdata> blockData; //blockData.resize(nblocks.width*nblocks.height); //pixData.resize(rawBlockSize*3); BlockData blockData[21]; // 1个blockData 存储一个block的信息,总共有 多少个block呢,(32-16)/8(stride) +1 *.... PixData pixData[16*16*3]; //1个pixData存储一个像素点的信息,一个block中有16*16个像素点,这里多分配了3份的block,用于计算方便 }myHOGCache; /******************************HOG算法需要的全局变量 ****************/ myHOGDescriptor descriptor; //HOG描述,含初始化变量信息 //descriptor.winSize.height=33; myHOGCache hogcache; //HOG缓存结构,精简算法需要的缓存偏移量信息 int width=32; int height=64; //float* grad = (float*)malloc( sizeof(float)*width*height*2); //unsigned char* qangle = (unsigned char*)malloc(sizeof(unsigned char)*width*height*2); float* grad ; unsigned char* qangle ; /******************************HOG算法需要的全局变量 ****************/ void normalizeBlockHistogram(float* _hist) { float* hist = &_hist[0]; size_t i, sz = hogcache.blockHistogramSize; float sum = 0; for( i = 0; i < sz; i++ ) sum += hist[i]*hist[i]; float scale = 1.f/(sqrt(sum)+sz*0.1f), thresh = (float)descriptor.L2HysThreshold; for( i = 0, sum = 0; i < sz; i++ ) { hist[i] = min2(hist[i]*scale, thresh); sum += hist[i]*hist[i]; } scale = 1.f/(sqrt(sum)+1e-3f); for( i = 0; i < sz; i++ ) hist[i] *= scale; } float* getBlock(myPoint pt, float* buf) { float* blockHist = buf; assert(&descriptor != 0); mySize blockSize = descriptor.blockSize; // pt += imgoffset; //所有情况偏移都为0 ,计算是没有做边界扩张 , assert( (unsigned)pt.x <= (unsigned)(width - blockSize.width) && (unsigned)pt.y <= (unsigned)(height - blockSize.height) ); int k, C1 = hogcache.count1, C2 = hogcache.count2, C4 = hogcache.count4; const float* gradPtr = (const float*)(grad + width*2*pt.y)+ pt.x*2; const unsigned char* qanglePtr = qangle+ width*2*pt.y +pt.x*2; assert(blockHist != 0); for( k = 0; k < hogcache.blockHistogramSize; k++ ) blockHist[k] = 0.f; const PixData* _pixData = &hogcache.pixData[0]; for( k = 0; k < C1; k++ ) { const PixData* pk = &_pixData[k]; const float* a = gradPtr + pk->gradOfs; float w = pk->gradWeight*pk->histWeights[0]; const uchar* h = qanglePtr + pk->qangleOfs; int h0 = h[0], h1 = h[1]; float* hist = blockHist + pk->histOfs[0]; float t0 = hist[h0] + a[0]*w; float t1 = hist[h1] + a[1]*w; hist[h0] = t0; hist[h1] = t1; int tst =0; } for( ; k < C2; k++ ) { const PixData* pk = &_pixData[k]; const float* a = gradPtr + pk->gradOfs; float w, t0, t1, a0 = a[0], a1 = a[1]; const uchar* h = qanglePtr + pk->qangleOfs; int h0 = h[0], h1 = h[1]; float* hist = blockHist + pk->histOfs[0]; w = pk->gradWeight*pk->histWeights[0]; t0 = hist[h0] + a0*w; t1 = hist[h1] + a1*w; hist[h0] = t0; hist[h1] = t1; hist = blockHist + pk->histOfs[1]; w = pk->gradWeight*pk->histWeights[1]; t0 = hist[h0] + a0*w; t1 = hist[h1] + a1*w; hist[h0] = t0; hist[h1] = t1; } for( ; k < C4; k++ ) { const PixData* pk = &_pixData[k]; const float* a = gradPtr + pk->gradOfs; float w, t0, t1, a0 = a[0], a1 = a[1]; const uchar* h = qanglePtr + pk->qangleOfs; int h0 = h[0], h1 = h[1]; float* hist = blockHist + pk->histOfs[0]; w = pk->gradWeight*pk->histWeights[0]; t0 = hist[h0] + a0*w; t1 = hist[h1] + a1*w; hist[h0] = t0; hist[h1] = t1; hist = blockHist + pk->histOfs[1]; w = pk->gradWeight*pk->histWeights[1]; t0 = hist[h0] + a0*w; t1 = hist[h1] + a1*w; hist[h0] = t0; hist[h1] = t1; hist = blockHist + pk->histOfs[2]; w = pk->gradWeight*pk->histWeights[2]; t0 = hist[h0] + a0*w; t1 = hist[h1] + a1*w; hist[h0] = t0; hist[h1] = t1; hist = blockHist + pk->histOfs[3]; w = pk->gradWeight*pk->histWeights[3]; t0 = hist[h0] + a0*w; t1 = hist[h1] + a1*w; hist[h0] = t0; hist[h1] = t1; } // float hist_show[36]; // memcpy(hist_show,blockHist,36*4); normalizeBlockHistogram(blockHist); // float hist_show2[36]; // memcpy(hist_show2,blockHist,36*4); return blockHist; } size_t getDescriptorSize(mySize winSize, mySize blockSize, mySize cellSize, mySize blockStride,size_t nbins) { assert(blockSize.width % cellSize.width == 0 && blockSize.height % cellSize.height == 0); assert((winSize.width - blockSize.width) % blockStride.width == 0 && (winSize.height - blockSize.height) % blockStride.height == 0 ); return (size_t)nbins* (blockSize.width/cellSize.width)* (blockSize.height/cellSize.height)* ((winSize.width - blockSize.width)/blockStride.width + 1)* ((winSize.height - blockSize.height)/blockStride.height + 1); } static float myfastAtan2( float y, float x ) { float angle; float scale = (float)(CV_PI/180); float ax = abs(x), ay = abs(y); float a, c, c2; if( ax >= ay ) { c = ay/(ax + (float)DBL_EPSILON); c2 = c*c; a = (((atan2_p7*c2 + atan2_p5)*c2 + atan2_p3)*c2 + atan2_p1)*c; } else { c = ax/(ay + (float)DBL_EPSILON); c2 = c*c; a = 90.f - (((atan2_p7*c2 + atan2_p5)*c2 + atan2_p3)*c2 + atan2_p1)*c; } if( x < 0 ) a = 180.f - a; if( y < 0 ) a = 360.f - a; angle = (float)(a*scale); return angle; } void cartToPolar( float dx, float dy,float* mag , float* angle ) { *mag = sqrt((float)(dy*dy + dx*dx)); *angle = myfastAtan2(dy, dx); } enum { BORDER_REPLICATE=IPL_BORDER_REPLICATE, BORDER_CONSTANT=IPL_BORDER_CONSTANT, BORDER_REFLECT=IPL_BORDER_REFLECT, BORDER_WRAP=IPL_BORDER_WRAP, BORDER_REFLECT_101=IPL_BORDER_REFLECT_101, BORDER_REFLECT101=BORDER_REFLECT_101, BORDER_TRANSPARENT=IPL_BORDER_TRANSPARENT, BORDER_DEFAULT=BORDER_REFLECT_101, BORDER_ISOLATED=16 }; int borderInterpolate2( int p, int len, int borderType ) { if( (unsigned)p < (unsigned)len ) ; else if( borderType == BORDER_REPLICATE ) p = p < 0 ? 0 : len - 1; else if( borderType == BORDER_REFLECT || borderType == BORDER_REFLECT_101 ) { int delta = borderType == BORDER_REFLECT_101; if( len == 1 ) return 0; do { if( p < 0 ) p = -p - 1 + delta; else p = len - 1 - (p - len) - delta; } while( (unsigned)p >= (unsigned)len ); } else if( borderType == BORDER_WRAP ) { if( p < 0 ) p -= ((p-len+1)/len)*len; if( p >= len ) p %= len; } else if( borderType == BORDER_CONSTANT ) p = -1; else printf( "Unknown/unsupported border type\n" ); return p; } //计算梯度(grad)和bin归属(qangle) grad 为双通道数据,两个字节为一组存储,分别存储分配到相邻bin的mod值,qangle也是如此, // 存储根据当前像素的角度值所划分的相邻bin的序号 ,与opencv一致 void computeGradient(const unsigned char* pImg , int width, int height, float* grad ,unsigned char* qangle ) { //不做gamma矫正 // 1,分别计算x和y方向的梯度 ,梯度公式: sqrt(dx*dx + dy*dy) 角度公式:arctan (dy/dx) int i, x, y; float* dbuf ; dbuf= (float*)malloc( sizeof(float)*width*4*8*88); if(dbuf ==NULL) { return ; } int b = sizeof(dbuf); float* Dx = dbuf; float* Dy = dbuf + width; float* Mag = dbuf +width*2; float* Angle = dbuf + width*3; int _nbins = nbins; float angleScale = (float)(_nbins/CV_PI); int* mapbuf = (int*)malloc(sizeof(int)*(width+height+4)); //int mapbuf[1000]; int* xmap = (int*)mapbuf +1; int* ymap = xmap + width +2; const int borderType = (int)BORDER_REFLECT_101; //边缘扩展xmap[-1] xmap[0] .....xmap[10] = 1 (0 1 2 3 ...8 9) 8 for( x = -1; x < width + 1; x++ ) xmap[x] = borderInterpolate2(x ,width, borderType); for( y = -1; y < height + 1; y++ ) ymap[y] = borderInterpolate2(y ,height, borderType); for( y=0; y<height; y++) { //行指针 int tmepmap = width*ymap[y]; const uchar* imgPtr = pImg + width*ymap[y]; const uchar* prevPtr = pImg + width*ymap[y-1]; const uchar* nextPtr = pImg + width*ymap[y+1]; //最后保存数据的行指针 float* gradPtr = (float*)(grad +y*width*2) ; //双通道的//Returns a pointer to the specified matrix row. uchar* qanglePtr = (uchar*)(qangle + y*width*2) ; //计算水平梯度和垂直梯度 for( x=0; x<width; x++) { int x1 = xmap[x]; //dbuf[x] = (float)(*(pImg +y*width +xmap[x+1]) - *(pImg +y*width +xmap[x-1) ); //dbuf[width +x] = (float)( *(pImg + (y+1)*width +x) - *(pImg + (y-1)*width +x)); /// //uchar c1= imgPtr[xmap[x+1]]; //uchar c2=imgPtr[xmap[x-1]]; // float temp = float(c1-c2); // uchar c11= nextPtr[x1]; // uchar c22= prevPtr[x1]; // float temp2 = float(c11-c22); / dbuf[x] = (float)(imgPtr[xmap[x+1]] - imgPtr[xmap[x-1]]); //dbuf length: width*4 dbuf[width + x] = (float)(nextPtr[x1] - prevPtr[x1]); // float tempMag=0; float tempAngle=0; cartToPolar( dbuf[x], dbuf[width + x], &tempMag, &tempAngle ); // cartToPolar( temp, temp2, &tempMag, &tempAngle ); Mag[x] = tempMag; Angle[x] = tempAngle; } for( x = 0; x < width; x++ ) { //保存该梯度方向在左右相邻的bin的模,本来只有一个模何来的两个?插值! //线性插值,比如某点算出来应该属于 bin 7.6,但是我们的bin都是整数的,四舍五入,把他划分到bin 8又太粗糙了 //那就按该点到bin7,bin8的距离分配,这样部分属于8,部分属于7。 float mag = dbuf[x+width*2], angle = dbuf[x+width*3]*angleScale - 0.5f; // 每一格 pi/9, 那现在算 t落在哪一格自然是 t/(pi/9) int hidx = floor(angle); //向下取整 angle -= hidx; gradPtr[x*2] = mag*(1.f - angle); //binx的大小是梯度方向和模的共同体现 gradPtr[x*2+1] = mag*angle; if( hidx < 0 ) hidx += _nbins; else if( hidx >= _nbins ) hidx -= _nbins; assert( (unsigned)hidx < (unsigned)_nbins ); //保存与该梯度方向相邻的左右两个bin编号 qanglePtr[x*2] = (uchar)hidx; //也是向下取整 hidx++; hidx &= hidx < _nbins ? -1 : 0; // hidx &= ( (hidx < _nbins ) ? -1 : 0;),如果hidx < nbins good;如果超过了,就算子bin 0 ;-1的补码是全1 qanglePtr[x*2+1] = (uchar)hidx; } } //int test =sizeof(dbuf); free(dbuf); dbuf=NULL; free(mapbuf); mapbuf=NULL; } void init(const unsigned char* pImg) { computeGradient( pImg , width, height, grad , qangle ); //保存梯度和bin到grad和qangle //计算权重 mySize blockSize; blockSize=descriptor.blockSize; mySize cellSize = descriptor.cellSize; mySize blockStride = descriptor.blockStride; int i,j, nbins= descriptor.nbins; int rawBlockSize = blockSize.width*blockSize.height; hogcache.nblocks.width= (hogcache.winSize.width - blockSize.width)/blockStride.width + 1; hogcache.nblocks.height= (hogcache.winSize.height - blockSize.height)/blockStride.height + 1; hogcache.ncells.width = blockSize.width/cellSize.width; hogcache.ncells.height = blockSize.height/cellSize.height; hogcache.blockHistogramSize = hogcache.ncells.width*hogcache.ncells.height*nbins; mySize ncells = hogcache.ncells; //分配权值内存,大小为block的区域大小 float* weights = (float*)malloc( sizeof(float)*blockSize.width*blockSize.height); float sigma = descriptor.winSigma >= 0 ? descriptor.winSigma : (blockSize.width + blockSize.height)/8.; float scale = 1.f/(sigma*sigma*2); //高斯权重分配到weights for(i=0; i<blockSize.height; i++) { for(j=0; j<blockSize.width; j++) { float di = i - blockSize.height*0.5f; float dj = j - blockSize.width*0.5f; weights[i*blockSize.width +j] = exp(-(di*di + dj*dj)*scale); } } //blockData.resize(nblocks.width*nblocks.height); //pixData.resize(rawBlockSize*3); int count1 =0, count2=0, count4 = 0; for(j=0; j<blockSize.width; j++) for(i=0; i<blockSize.height; i++) { PixData* data = 0; float cellX = (j+0.5f)/cellSize.width - 0.5f; float cellY = (i+0.5f)/cellSize.height - 0.5f; int icellX0 = floor(cellX); int icellY0 = floor(cellY); int icellX1 = icellX0 + 1, icellY1 = icellY0 + 1; cellX -= icellX0; cellY -= icellY0; if( (unsigned)icellX0 < (unsigned)ncells.width && (unsigned)icellX1 < (unsigned)ncells.width ) { if( (unsigned)icellY0 < (unsigned)ncells.height && (unsigned)icellY1 < (unsigned)ncells.height ) { data = &hogcache.pixData[rawBlockSize*2 + (count4++)]; data->histOfs[0] = (icellX0*ncells.height + icellY0)*nbins; data->histWeights[0] = (1.f - cellX)*(1.f - cellY); data->histOfs[1] = (icellX1*ncells.height + icellY0)*nbins; data->histWeights[1] = cellX*(1.f - cellY); data->histOfs[2] = (icellX0*ncells.height + icellY1)*nbins; data->histWeights[2] = (1.f - cellX)*cellY; data->histOfs[3] = (icellX1*ncells.height + icellY1)*nbins; data->histWeights[3] = cellX*cellY; } else { data = &hogcache.pixData[rawBlockSize + (count2++)]; if( (unsigned)icellY0 < (unsigned)ncells.height ) { icellY1 = icellY0; cellY = 1.f - cellY; } data->histOfs[0] = (icellX0*ncells.height + icellY1)*nbins; data->histWeights[0] = (1.f - cellX)*cellY; data->histOfs[1] = (icellX1*ncells.height + icellY1)*nbins; data->histWeights[1] = cellX*cellY; data->histOfs[2] = data->histOfs[3] = 0; data->histWeights[2] = data->histWeights[3] = 0; } } else { if( (unsigned)icellX0 < (unsigned)ncells.width ) { icellX1 = icellX0; cellX = 1.f - cellX; } if( (unsigned)icellY0 < (unsigned)ncells.height && (unsigned)icellY1 < (unsigned)ncells.height ) { data = &hogcache.pixData[rawBlockSize + (count2++)]; data->histOfs[0] = (icellX1*ncells.height + icellY0)*nbins; data->histWeights[0] = cellX*(1.f - cellY); data->histOfs[1] = (icellX1*ncells.height + icellY1)*nbins; data->histWeights[1] = cellX*cellY; data->histOfs[2] = data->histOfs[3] = 0; data->histWeights[2] = data->histWeights[3] = 0; } else { data = &hogcache.pixData[count1++]; if( (unsigned)icellY0 < (unsigned)ncells.height ) { icellY1 = icellY0; cellY = 1.f - cellY; } data->histOfs[0] = (icellX1*ncells.height + icellY1)*nbins; data->histWeights[0] = cellX*cellY; data->histOfs[1] = data->histOfs[2] = data->histOfs[3] = 0; data->histWeights[1] = data->histWeights[2] = data->histWeights[3] = 0; } } /* data->gradOfs = (grad.cols*i + j)*2; data->qangleOfs = (qangle.cols*i + j)*2; data->gradWeight = weights(i,j); */ data->gradOfs = (width*i + j)*2; data->qangleOfs = (width*i + j)*2; // data->gradWeight = weights(i,j); //weights的大小和block一样 data->gradWeight = weights[i*blockSize.width +j]; } assert( count1 + count2 + count4 == rawBlockSize ); //defragment pixData for( j=0; j<count2; j++) { hogcache.pixData[j + count1] = hogcache.pixData[j + rawBlockSize]; } for( j = 0; j < count4; j++ ) { hogcache.pixData[j + count1 + count2] = hogcache.pixData[j + rawBlockSize*2]; } count2 += count1; count4 += count2; hogcache.count1 = count1; hogcache.count2 = count2; hogcache.count4 = count4; // initialize blockData for( j = 0; j < hogcache.nblocks.width; j++ ) for( i = 0; i < hogcache.nblocks.height; i++ ) { BlockData *data = &hogcache.blockData[j*hogcache.nblocks.height + i]; data->histOfs = (j*hogcache.nblocks.height + i)*hogcache.blockHistogramSize; data->imgOffset.x= j*blockStride.width; data->imgOffset.y = i*blockStride.height; } free(weights); weights=NULL; } //返回特征描述向量descriptors,函数内会给descriptors分配好内存,并且返回这个内存的首地址。 float* compute(const unsigned char* pImg, mySize winStride ) { winStride = descriptor.cellSize; init(pImg); /* float grad_show[32*64] ; memcpy(grad_show,grad,32*64*4); unsigned char qangle_show[32*64*2] ; memcpy(qangle_show,qangle,32*64*2); */ //nwindows 表示一张图片中有多少个win 这里特征提取时选取时一个图片就是一个win ,所以只有1个win size_t nwindows =1; //if( !nwindows ) // nwindows = cache.windowsInImage(paddedImgSize, winStride).area(); BlockData* blockData = &hogcache.blockData[0]; int nblocks = hogcache.nblocks.width*hogcache.nblocks.height; //init时候已经计算好 int blockHistogramSize = hogcache.blockHistogramSize; // size_t getDescriptorSize(mySize winSize, mySize blockSize, mySize cellSize, mySize blockStride,size_t nbins) size_t dsize = getDescriptorSize( hogcache.winSize, descriptor.blockSize, descriptor.cellSize, descriptor.blockStride, nbins); float* descriptors; //descriptors.resize(dsize*nwindows); descriptors = (float*)malloc(sizeof(float)*dsize*nwindows); int i=0; for(i=0; i<nwindows; i++) { float* descriptor_temp =&descriptors[i*dsize]; /* 只有一个win的时候,忽略下面代码, myPoint pt0; if( !locations.empty() ) { pt0 = locations[i]; if( pt0.x < -padding.width || pt0.x > img.cols + padding.width - winSize.width || pt0.y < -padding.height || pt0.y > img.rows + padding.height - winSize.height ) continue; } else { pt0 = cache.getWindow(paddedImgSize, winStride, (int)i).tl() - Point(padding); CV_Assert(pt0.x % cacheStride.width == 0 && pt0.y % cacheStride.height == 0); } */ int j=0; for( j=0; j<nblocks; j++) { BlockData bj = blockData[j]; //Point pt = pt0 + bj.imgOffset; myPoint pt; pt.x = bj.imgOffset.x; pt.y = bj.imgOffset.y; float* dst = descriptor_temp +bj.histOfs; const float* src = getBlock(pt,dst); if(src != dst) { int k; for( k=0; k<blockHistogramSize; k++) { dst[k] = src[k]; } } } } return descriptors; } /* int main() { grad = (float*)malloc( sizeof(float)*width*height*2); qangle = (unsigned char*)malloc(sizeof(unsigned char)*width*height*2); string ImgName; ImgName = "E:\\\\pos_0.png"; Mat src = imread(ImgName);//读取图片 Mat src_gray; //结构初始化 descriptor.winSize.width=32; descriptor.winSize.height=64; descriptor.blockSize.width = 16; descriptor.blockSize.height =16; descriptor.blockStride.width =8; descriptor.blockStride.height =8; descriptor.cellSize.width = 8; descriptor.cellSize.height = 8; descriptor.nbins=9; descriptor.winSigma= -2; descriptor.L2HysThreshold =0.2; hogcache.winSize.width=32; hogcache.winSize.height=64; cvtColor(src,src_gray,CV_BGR2GRAY); uchar* pImg_data = src_gray.data; for(int i=0; i<src_gray.rows;i++) { for(int j=0; j<src_gray.cols;j++) { uchar temp = pImg_data[i*src_gray.cols+j]; printf("%4d",temp); } printf("\n"); printf("\n"); } mySize winStride1 ={0,0}; float* descriptor_out; descriptor_out = compute((unsigned char*)pImg_data, winStride1 ); float descriptor_show[10000]; memcpy(descriptor_show,descriptor_out,10000); return 0; }*/ </blockdata></pixdata></fstream></iostream>