HOG C实现，验证通过

#include"string.h"#include"assert.h"#include"stdlib.h"#include"math.h"#include"stdio.h"//#include <iostream> //#include <fstream> #define CV_PI3.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);//高斯权重分配到weightsfor(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>