// compute gradient magnitude and orientation at each location (uses sse)
void gradMag( float *I, float *M, float *O, int h, int w, int d ) {
int x, y, y1, c, h4, s; float *Gx, *Gy, *M2; __m128 *_Gx, *_Gy, *_M2, _m;
float *acost = acosTable(), acMult=25000/2.02f;
// allocate memory for storing one column of output (padded so h4%4==0)
h4=(h%4==0) ? h : h-(h%4)+4; s=d*h4*sizeof(float);
M2=(float*) alMalloc(s,16); _M2=(__m128*) M2;
Gx=(float*) alMalloc(s,16); _Gx=(__m128*) Gx;
Gy=(float*) alMalloc(s,16); _Gy=(__m128*) Gy;
// compute gradient magnitude and orientation for each column
for( x=0; x<w; x++ ) {
// compute gradients (Gx, Gy) and squared magnitude (M2) for each channel
for( c=0; c<d; c++ ) grad1( I+x*h+c*w*h, Gx+c*h4, Gy+c*h4, h, w, x );
for( y=0; y<d*h4/4; y++ ) _M2[y]=ADD(MUL(_Gx[y],_Gx[y]),MUL(_Gy[y],_Gy[y]));
// store gradients with maximum response in the first channel
for(c=1; c<d; c++) {
for( y=0; y<h4/4; y++ ) {
y1=h4/4*c+y; _m = CMPGT( _M2[y1], _M2[y] );
_M2[y] = OR( AND(_m,_M2[y1]), ANDNOT(_m,_M2[y]) );
_Gx[y] = OR( AND(_m,_Gx[y1]), ANDNOT(_m,_Gx[y]) );
_Gy[y] = OR( AND(_m,_Gy[y1]), ANDNOT(_m,_Gy[y]) );
}
}
// compute gradient mangitude (M) and normalize Gx
for( y=0; y<h4/4; y++ ) {
_m = MIN( RCPSQRT(_M2[y]), SET(1e10f) );
_M2[y] = RCP(_m);
_Gx[y] = MUL( MUL(_Gx[y],_m), SET(acMult) );
_Gx[y] = XOR( _Gx[y], AND(_Gy[y], SET(-0.f)) );
};
memcpy( M+x*h, M2, h*sizeof(float) );
// compute and store gradient orientation (O) via table lookup
if(O!=0) for( y=0; y<h; y++ ) O[x*h+y] = acost[(int)Gx[y]];
}
alFree(Gx); alFree(Gy); alFree(M2);
}
// compute gradient magnitude and orientation at each location (uses sse)
void gradMag( float *I, float *M, float *O, int h, int w, int d, bool full ) {
int x, y, y1, c, h4, s;
float *Gx, *Gy, *M2;
__m128 *_Gx, *_Gy, *_M2, _m;
float *acost = acosTable(), acMult = 10000.0f;
// allocate memory for storing one column of output (padded so h4%4==0)
h4 = (h % 4 == 0) ? h : h - (h % 4) + 4;
s = d * h4 * sizeof(float);
M2 = (float*) alMalloc(s, 16);
_M2 = (__m128*) M2;
Gx = (float*) alMalloc(s, 16);
_Gx = (__m128*) Gx;
Gy = (float*) alMalloc(s, 16);
_Gy = (__m128*) Gy;
// compute gradient magnitude and orientation for each column
for ( x = 0; x < w; x++ ) {
// compute gradients (Gx, Gy) with maximum squared magnitude (M2)
for (c = 0; c < d; c++) {
grad1( I + x * h + c * w * h, Gx + c * h4, Gy + c * h4, h, w, x );
for ( y = 0; y < h4 / 4; y++ ) {
y1 = h4 / 4 * c + y;
_M2[y1] = ADD(MUL(_Gx[y1], _Gx[y1]), MUL(_Gy[y1], _Gy[y1]));
if ( c == 0 ) { continue; }
_m = CMPGT( _M2[y1], _M2[y] );
_M2[y] = OR( AND(_m, _M2[y1]), ANDNOT(_m, _M2[y]) );
_Gx[y] = OR( AND(_m, _Gx[y1]), ANDNOT(_m, _Gx[y]) );
_Gy[y] = OR( AND(_m, _Gy[y1]), ANDNOT(_m, _Gy[y]) );
}
}
// compute gradient mangitude (M) and normalize Gx
for ( y = 0; y < h4 / 4; y++ ) {
_m = MINsse( RCPSQRT(_M2[y]), SET(1e10f) );
_M2[y] = RCP(_m);
if (O) { _Gx[y] = MUL( MUL(_Gx[y], _m), SET(acMult) ); }
if (O) { _Gx[y] = XOR( _Gx[y], AND(_Gy[y], SET(-0.f)) ); }
};
memcpy( M + x * h, M2, h * sizeof(float) );
// compute and store gradient orientation (O) via table lookup
if ( O != 0 ) for ( y = 0; y < h; y++ ) { O[x * h + y] = acost[(int)Gx[y]]; }
if ( O != 0 && full ) {
y1 = ((~size_t(O + x * h) + 1) & 15) / 4;
y = 0;
for ( ; y < y1; y++ ) { O[y + x * h] += (Gy[y] < 0) * PI; }
for ( ; y < h - 4; y += 4 ) STRu( O[y + x * h],
ADD( LDu(O[y + x * h]), AND(CMPLT(LDu(Gy[y]), SET(0.f)), SET(PI)) ) );
for ( ; y < h; y++ ) { O[y + x * h] += (Gy[y] < 0) * PI; }
}
}
alFree(Gx);
alFree(Gy);
alFree(M2);
}
// convolve I by a 2rx1 triangle filter (uses SSE)
void convTri( float *I, float *O, int h, int w, int d, int r, int s ) {
r++; float nrm = 1.0f/(r*r*r*r); int i, j, k=(s-1)/2, h0, h1, w0;
if(h%4==0) h0=h1=h; else { h0=h-(h%4); h1=h0+4; } w0=(w/s)*s;
float *T=(float*) alMalloc(2*h1*sizeof(float),16), *U=T+h1;
while(d-- > 0) {
// initialize T and U
for(j=0; j<h0; j+=4) STR(U[j], STR(T[j], LDu(I[j])));
for(i=1; i<r; i++) for(j=0; j<h0; j+=4) INC(U[j],INC(T[j],LDu(I[j+i*h])));
for(j=0; j<h0; j+=4) STR(U[j],MUL(nrm,(SUB(MUL(2,LD(U[j])),LD(T[j])))));
for(j=0; j<h0; j+=4) STR(T[j],0);
for(j=h0; j<h; j++ ) U[j]=T[j]=I[j];
for(i=1; i<r; i++) for(j=h0; j<h; j++ ) U[j]+=T[j]+=I[j+i*h];
for(j=h0; j<h; j++ ) { U[j] = nrm * (2*U[j]-T[j]); T[j]=0; }
// prepare and convolve each column in turn
k++; if(k==s) { k=0; convTriY(U,O,h,r-1,s); O+=h/s; }
for( i=1; i<w0; i++ ) {
float *Il=I+(i-1-r)*h; if(i<=r) Il=I+(r-i)*h; float *Im=I+(i-1)*h;
float *Ir=I+(i-1+r)*h; if(i>w-r) Ir=I+(2*w-r-i)*h;
for( j=0; j<h0; j+=4 ) {
INC(T[j],ADD(LDu(Il[j]),LDu(Ir[j]),MUL(-2,LDu(Im[j]))));
INC(U[j],MUL(nrm,LD(T[j])));
}
for( j=h0; j<h; j++ ) U[j]+=nrm*(T[j]+=Il[j]+Ir[j]-2*Im[j]);
k++; if(k==s) { k=0; convTriY(U,O,h,r-1,s); O+=h/s; }
}
I+=w*h;
}
alFree(T);
}
// convolve I by a 2rx1 max filter
void convMax( float *I, float *O, int h, int w, int d, int r ) {
if( r>w-1 ) r=w-1; if( r>h-1 ) r=h-1; int m=2*r+1;
float *T=(float*) alMalloc(m*2*sizeof(float),16);
for( int d0=0; d0<d; d0++ ) for( int x=0; x<w; x++ ) {
float *Oc=O+d0*h*w+h*x, *Ic=I+d0*h*w+h*x;
convMaxY(Ic,Oc,T,h,r);
}
alFree(T);
}
// convolve I by a [1 1; 1 1] filter (uses SSE)
void conv11( float *I, float *O, int h, int w, int d, int side, int s ) {
const float nrm = 0.25f; int i, j;
float *I0, *I1, *T = (float*) alMalloc(h*sizeof(float),16);
for( int d0=0; d0<d; d0++ ) for( i=s/2; i<w; i+=s ) {
I0=I1=I+i*h+d0*h*w; if(side%2) { if(i<w-1) I1+=h; } else { if(i) I0-=h; }
for( j=0; j<h-4; j+=4 ) STR( T[j], MUL(nrm,ADD(LDu(I0[j]),LDu(I1[j]))) );
for( ; j<h; j++ ) T[j]=nrm*(I0[j]+I1[j]);
conv11Y(T,O,h,side,s); O+=h/s;
}
alFree(T);
}
// convolve I by a [1 p 1] filter (uses SSE)
void convTri1( float *I, float *O, int h, int w, int d, float p, int s ) {
const float nrm = 1.0f/((p+2)*(p+2)); int i, j, h0=h-(h%4);
float *Il, *Im, *Ir, *T=(float*) alMalloc(h*sizeof(float),16);
for( int d0=0; d0<d; d0++ ) for( i=s/2; i<w; i+=s ) {
Il=Im=Ir=I+i*h+d0*h*w; if(i>0) Il-=h; if(i<w-1) Ir+=h;
for( j=0; j<h0; j+=4 )
STR(T[j],MUL(nrm,ADD(ADD(LDu(Il[j]),MUL(p,LDu(Im[j]))),LDu(Ir[j]))));
for( j=h0; j<h; j++ ) T[j]=nrm*(Il[j]+p*Im[j]+Ir[j]);
convTri1Y(T,O,h,p,s); O+=h/s;
}
alFree(T);
}
ALeffectState *ModulatorCreate(void)
{
ALmodulatorState *state;
state = alMalloc(sizeof(*state));
if(!state)
return NULL;
state->state.Destroy = ModulatorDestroy;
state->state.DeviceUpdate = ModulatorDeviceUpdate;
state->state.Update = ModulatorUpdate;
state->state.Process = ModulatorProcess;
state->index = 0.0f;
state->step = 1.0f;
state->iirFilter.coeff = 0.0f;
state->iirFilter.history[0] = 0.0f;
return &state->state;
}
// convolve I by a 2r+1 x 2r+1 ones filter (uses SSE)
void convBox( float *I, float *O, int h, int w, int d, int r, int s ) {
float nrm = 1.0f/((2*r+1)*(2*r+1)); int i, j, k=(s-1)/2, h0, h1, w0; // s=1
if(h%4==0) h0=h1=h; else { h0=h-(h%4); h1=h0+4; } w0=(w/s)*s;
float *T=(float*) alMalloc(h1*sizeof(float),16);
while(d-- > 0) {
// initialize T
memset( T, 0, h1*sizeof(float) );
for(i=0; i<=r; i++) for(j=0; j<h0; j+=4) INC(T[j],LDu(I[j+i*h]));
for(j=0; j<h0; j+=4) STR(T[j],MUL(nrm,SUB(MUL(2,LD(T[j])),LDu(I[j+r*h]))));
for(i=0; i<=r; i++) for(j=h0; j<h; j++ ) T[j]+=I[j+i*h]; // assemble just perform like following 2 lines
for(j=h0; j<h; j++ ) T[j]=nrm*(2*T[j]-I[j+r*h]);
// prepare and convolve each column in turn
k++; if(k==s) { k=0; convBoxY(T,O,h,r,s); O+=h/s; }
for( i=1; i<w0; i++ ) {
float *Il=I+(i-1-r)*h; if(i<=r) Il=I+(r-i)*h;
float *Ir=I+(i+r)*h; if(i>=w-r) Ir=I+(2*w-r-i-1)*h;
for(j=0; j<h0; j+=4) DEC(T[j],MUL(nrm,SUB(LDu(Il[j]),LDu(Ir[j]))));
for(j=h0; j<h; j++ ) T[j]-=nrm*(Il[j]-Ir[j]);
k++; if(k==s) { k=0; convBoxY(T,O,h,r,s); O+=h/s; }
}
I+=w*h;
}
alFree(T);
}
// compute nOrients gradient histograms per bin x bin block of pixels
void gradHist( float *M, float *O, float *H, int h, int w,
int bin, int nOrients, int softBin, bool full )
{
const int hb=h/bin, wb=w/bin, h0=hb*bin, w0=wb*bin, nb=wb*hb;
const float s=(float)bin, sInv=1/s, sInv2=1/s/s;
float *H0, *H1, *M0, *M1; int x, y; int *O0, *O1; float xb, init;
O0=(int*)alMalloc(h*sizeof(int),16); M0=(float*) alMalloc(h*sizeof(float),16);
O1=(int*)alMalloc(h*sizeof(int),16); M1=(float*) alMalloc(h*sizeof(float),16);
// main loop
for( x=0; x<w0; x++ ) {
// compute target orientation bins for entire column - very fast
gradQuantize(O+x*h,M+x*h,O0,O1,M0,M1,nb,h0,sInv2,nOrients,full,softBin>=0);
if( softBin<0 && softBin%2==0 ) {
// no interpolation w.r.t. either orienation or spatial bin
H1=H+(x/bin)*hb;
#define GH H1[O0[y]]+=M0[y]; y++;
if( bin==1 ) for(y=0; y<h0;) { GH; H1++; }
else if( bin==2 ) for(y=0; y<h0;) { GH; GH; H1++; }
else if( bin==3 ) for(y=0; y<h0;) { GH; GH; GH; H1++; }
else if( bin==4 ) for(y=0; y<h0;) { GH; GH; GH; GH; H1++; }
else for( y=0; y<h0;) { for( int y1=0; y1<bin; y1++ ) { GH; } H1++; }
#undef GH
} else if( softBin%2==0 || bin==1 ) {
// interpolate w.r.t. orientation only, not spatial bin
H1=H+(x/bin)*hb;
#define GH H1[O0[y]]+=M0[y]; H1[O1[y]]+=M1[y]; y++;
if( bin==1 ) for(y=0; y<h0;) { GH; H1++; }
else if( bin==2 ) for(y=0; y<h0;) { GH; GH; H1++; }
else if( bin==3 ) for(y=0; y<h0;) { GH; GH; GH; H1++; }
else if( bin==4 ) for(y=0; y<h0;) { GH; GH; GH; GH; H1++; }
else for( y=0; y<h0;) { for( int y1=0; y1<bin; y1++ ) { GH; } H1++; }
#undef GH
} else {
// interpolate using trilinear interpolation
float ms[4], xyd, yb, xd, yd; __m128 _m, _m0, _m1;
bool hasLf, hasRt; int xb0, yb0;
if( x==0 ) { init=(0+.5f)*sInv-0.5f; xb=init; }
hasLf = xb>=0; xb0 = hasLf?(int)xb:-1; hasRt = xb0 < wb-1;
xd=xb-xb0; xb+=sInv; yb=init; y=0;
// macros for code conciseness
#define GHinit yd=yb-yb0; yb+=sInv; H0=H+xb0*hb+yb0; xyd=xd*yd; \
ms[0]=1-xd-yd+xyd; ms[1]=yd-xyd; ms[2]=xd-xyd; ms[3]=xyd;
#define GH(H,ma,mb) H1=H; STRu(*H1,ADD(LDu(*H1),MUL(ma,mb)));
// leading rows, no top bin
for( ; y<bin/2; y++ ) {
yb0=-1; GHinit;
if(hasLf) { H0[O0[y]+1]+=ms[1]*M0[y]; H0[O1[y]+1]+=ms[1]*M1[y]; }
if(hasRt) { H0[O0[y]+hb+1]+=ms[3]*M0[y]; H0[O1[y]+hb+1]+=ms[3]*M1[y]; }
}
// main rows, has top and bottom bins, use SSE for minor speedup
if( softBin<0 ) for( ; ; y++ ) {
yb0 = (int) yb; if(yb0>=hb-1) break; GHinit; _m0=SET(M0[y]);
if(hasLf) { _m=SET(0,0,ms[1],ms[0]); GH(H0+O0[y],_m,_m0); }
if(hasRt) { _m=SET(0,0,ms[3],ms[2]); GH(H0+O0[y]+hb,_m,_m0); }
} else for( ; ; y++ ) {
yb0 = (int) yb; if(yb0>=hb-1) break; GHinit;
_m0=SET(M0[y]); _m1=SET(M1[y]);
if(hasLf) { _m=SET(0,0,ms[1],ms[0]);
GH(H0+O0[y],_m,_m0); GH(H0+O1[y],_m,_m1); }
if(hasRt) { _m=SET(0,0,ms[3],ms[2]);
GH(H0+O0[y]+hb,_m,_m0); GH(H0+O1[y]+hb,_m,_m1); }
}
// final rows, no bottom bin
for( ; y<h0; y++ ) {
yb0 = (int) yb; GHinit;
if(hasLf) { H0[O0[y]]+=ms[0]*M0[y]; H0[O1[y]]+=ms[0]*M1[y]; }
if(hasRt) { H0[O0[y]+hb]+=ms[2]*M0[y]; H0[O1[y]+hb]+=ms[2]*M1[y]; }
}
#undef GHinit
#undef GH
}
}
alFree(O0); alFree(O1); alFree(M0); alFree(M1);
// normalize boundary bins which only get 7/8 of weight of interior bins
if( softBin%2!=0 ) for( int o=0; o<nOrients; o++ ) {
x=0; for( y=0; y<hb; y++ ) H[o*nb+x*hb+y]*=8.f/7.f;
y=0; for( x=0; x<wb; x++ ) H[o*nb+x*hb+y]*=8.f/7.f;
x=wb-1; for( y=0; y<hb; y++ ) H[o*nb+x*hb+y]*=8.f/7.f;
y=hb-1; for( x=0; x<wb; x++ ) H[o*nb+x*hb+y]*=8.f/7.f;
}
}
static ALCboolean wave_reset_playback(ALCdevice *device)
{
wave_data *data = (wave_data*)device->ExtraData;
ALuint channels=0, bits=0;
size_t val;
fseek(data->f, 0, SEEK_SET);
clearerr(data->f);
switch(device->FmtType)
{
case DevFmtByte:
device->FmtType = DevFmtUByte;
break;
case DevFmtUShort:
device->FmtType = DevFmtShort;
break;
case DevFmtUByte:
case DevFmtShort:
case DevFmtFloat:
break;
}
bits = BytesFromDevFmt(device->FmtType) * 8;
channels = ChannelsFromDevFmt(device->FmtChans);
fprintf(data->f, "RIFF");
fwrite32le(0xFFFFFFFF, data->f); // 'RIFF' header len; filled in at close
fprintf(data->f, "WAVE");
fprintf(data->f, "fmt ");
fwrite32le(40, data->f); // 'fmt ' header len; 40 bytes for EXTENSIBLE
// 16-bit val, format type id (extensible: 0xFFFE)
fwrite16le(0xFFFE, data->f);
// 16-bit val, channel count
fwrite16le(channels, data->f);
// 32-bit val, frequency
fwrite32le(device->Frequency, data->f);
// 32-bit val, bytes per second
fwrite32le(device->Frequency * channels * bits / 8, data->f);
// 16-bit val, frame size
fwrite16le(channels * bits / 8, data->f);
// 16-bit val, bits per sample
fwrite16le(bits, data->f);
// 16-bit val, extra byte count
fwrite16le(22, data->f);
// 16-bit val, valid bits per sample
fwrite16le(bits, data->f);
// 32-bit val, channel mask
fwrite32le(channel_masks[channels], data->f);
// 16 byte GUID, sub-type format
val = fwrite(((bits==32) ? SUBTYPE_FLOAT : SUBTYPE_PCM), 1, 16, data->f);
fprintf(data->f, "data");
fwrite32le(0xFFFFFFFF, data->f); // 'data' header len; filled in at close
if(ferror(data->f))
{
AL_PRINT("Error writing header: %s\n", strerror(errno));
return ALC_FALSE;
}
data->DataStart = ftell(data->f);
data->size = device->UpdateSize * channels * bits / 8;
data->buffer = alMalloc(data->size);
if(!data->buffer)
{
AL_PRINT("buffer malloc failed\n");
return ALC_FALSE;
}
SetDefaultWFXChannelOrder(device);
data->thread = StartThread(WaveProc, device);
if(data->thread == NULL)
{
alFree(data->buffer);
data->buffer = NULL;
return ALC_FALSE;
}
return ALC_TRUE;
}
void
pcl::people::HOG::gradMag( float *I, int h, int w, int d, float *M, float *O ) const
{
#if defined(__SSE2__)
int x, y, y1, c, h4, s; float *Gx, *Gy, *M2; __m128 *_Gx, *_Gy, *_M2, _m;
float *acost = acosTable(), acMult=25000/2.02f;
// allocate memory for storing one column of output (padded so h4%4==0)
h4=(h%4==0) ? h : h-(h%4)+4; s=d*h4*sizeof(float);
M2=(float*) alMalloc(s,16);
_M2=(__m128*) M2;
Gx=(float*) alMalloc(s,16); _Gx=(__m128*) Gx;
Gy=(float*) alMalloc(s,16); _Gy=(__m128*) Gy;
// compute gradient magnitude and orientation for each column
for( x=0; x<w; x++ ) {
// compute gradients (Gx, Gy) and squared magnitude (M2) for each channel
for( c=0; c<d; c++ ) grad1( I+x*h+c*w*h, Gx+c*h4, Gy+c*h4, h, w, x );
for( y=0; y<d*h4/4; y++ ) _M2[y]=pcl::sse_add(pcl::sse_mul(_Gx[y],_Gx[y]),pcl::sse_mul(_Gy[y],_Gy[y]));
// store gradients with maximum response in the first channel
for(c=1; c<d; c++) {
for( y=0; y<h4/4; y++ ) {
y1=h4/4*c+y; _m = pcl::sse_cmpgt( _M2[y1], _M2[y] );
_M2[y] = pcl::sse_or( pcl::sse_and(_m,_M2[y1]), pcl::sse_andnot(_m,_M2[y]) );
_Gx[y] = pcl::sse_or( pcl::sse_and(_m,_Gx[y1]), pcl::sse_andnot(_m,_Gx[y]) );
_Gy[y] = pcl::sse_or( pcl::sse_and(_m,_Gy[y1]), pcl::sse_andnot(_m,_Gy[y]) );
}
}
// compute gradient magnitude (M) and normalize Gx
for( y=0; y<h4/4; y++ ) {
_m = pcl::sse_min( pcl::sse_rcpsqrt(_M2[y]), pcl::sse_set(1e10f) );
_M2[y] = pcl::sse_rcp(_m);
_Gx[y] = pcl::sse_mul( pcl::sse_mul(_Gx[y],_m), pcl::sse_set(acMult) );
_Gx[y] = pcl::sse_xor( _Gx[y], pcl::sse_and(_Gy[y], pcl::sse_set(-0.f)) );
};
memcpy( M+x*h, M2, h*sizeof(float) );
// compute and store gradient orientation (O) via table lookup
if(O!=0) for( y=0; y<h; y++ ) O[x*h+y] = acost[(int)Gx[y]];
}
alFree(Gx); alFree(Gy); alFree(M2);
#else
int x, y, y1, c, h4, s; float *Gx, *Gy, *M2;
float *acost = acosTable(), acMult=25000/2.02f;
// allocate memory for storing one column of output (padded so h4%4==0)
h4=(h%4==0) ? h : h-(h%4)+4; s=d*h4*sizeof(float);
M2=(float*) alMalloc(s,16);
Gx=(float*) alMalloc(s,16);
Gy=(float*) alMalloc(s,16);
float m;
// compute gradient magnitude and orientation for each column
for( x=0; x<w; x++ ) {
// compute gradients (Gx, Gy) and squared magnitude (M2) for each channel
for( c=0; c<d; c++ ) grad1( I+x*h+c*w*h, Gx+c*h4, Gy+c*h4, h, w, x );
for( y=0; y<d*h4; y++ )
{
M2[y] = Gx[y] * Gx[y] + Gy[y] * Gy[y];
}
// store gradients with maximum response in the first channel
for(c=1; c<d; c++)
{
for( y=0; y<h4/4; y++ )
{
y1=h4/4*c+y;
for (int ii = 0; ii < 4; ++ii)
{
if (M2[y1 * 4 + ii] > M2[y * 4 + ii])
{
M2[y * 4 + ii] = M2[y1 * 4 + ii];
Gx[y * 4 + ii] = Gx[y1 * 4 + ii];
Gy[y * 4 + ii] = Gy[y1 * 4 + ii];
}
}
}
}
// compute gradient magnitude (M) and normalize Gx
for( y=0; y<h4; y++ )
{
m = 1.0f/sqrtf(M2[y]);
m = m < 1e10f ? m : 1e10f;
M2[y] = 1.0f / m;
Gx[y] = ((Gx[y] * m) * acMult);
if (Gy[y] < 0)
Gx[y] = -Gx[y];
}
memcpy( M+x*h, M2, h*sizeof(float) );
// compute and store gradient orientation (O) via table lookup
if(O!=0) for( y=0; y<h; y++ ) O[x*h+y] = acost[(int)Gx[y]];
}
alFree(Gx); alFree(Gy); alFree(M2);
#endif
}
void
pcl::people::HOG::gradHist( float *M, float *O, int h, int w, int bin_size, int n_orients, bool soft_bin, float *H ) const
{
const int hb=h/bin_size, wb=w/bin_size, h0=hb*bin_size, w0=wb*bin_size, nb=wb*hb;
const float s=(float)bin_size, sInv=1/s, sInv2=1/s/s;
float *H0, *H1, *M0, *M1; int x, y; int *O0, *O1;
O0=(int*)alMalloc(h*sizeof(int),16); M0=(float*) alMalloc(h*sizeof(float),16);
O1=(int*)alMalloc(h*sizeof(int),16); M1=(float*) alMalloc(h*sizeof(float),16);
// main loop
float xb = 0;
float init = 0;
for( x=0; x<w0; x++ ) {
// compute target orientation bins for entire column - very fast
gradQuantize( O+x*h, M+x*h, O0, O1, M0, M1, n_orients, nb, h0, sInv2 );
if( !soft_bin || bin_size==1 ) {
// interpolate w.r.t. orientation only, not spatial bin_size
H1=H+(x/bin_size)*hb;
#define GH H1[O0[y]]+=M0[y]; H1[O1[y]]+=M1[y]; y++;
if( bin_size==1 ) for(y=0; y<h0;) { GH; H1++; }
else if( bin_size==2 ) for(y=0; y<h0;) { GH; GH; H1++; }
else if( bin_size==3 ) for(y=0; y<h0;) { GH; GH; GH; H1++; }
else if( bin_size==4 ) for(y=0; y<h0;) { GH; GH; GH; GH; H1++; }
else for( y=0; y<h0;) { for( int y1=0; y1<bin_size; y1++ ) { GH; } H1++; }
#undef GH
} else {
// interpolate using trilinear interpolation
#if defined(__SSE2__)
float ms[4], xyd, yb, xd, yd; __m128 _m, _m0, _m1;
bool hasLf, hasRt; int xb0, yb0;
if( x==0 ) { init=(0+.5f)*sInv-0.5f; xb=init; }
hasLf = xb>=0; xb0 = hasLf?(int)xb:-1; hasRt = xb0 < wb-1;
xd=xb-xb0; xb+=sInv; yb=init; y=0;
// macros for code conciseness
#define GHinit yd=yb-yb0; yb+=sInv; H0=H+xb0*hb+yb0; xyd=xd*yd; \
ms[0]=1-xd-yd+xyd; ms[1]=yd-xyd; ms[2]=xd-xyd; ms[3]=xyd;
#define GH(H,ma,mb) H1=H; pcl::sse_stru(*H1,pcl::sse_add(pcl::sse_ldu(*H1),pcl::sse_mul(ma,mb)));
// leading rows, no top bin_size
for( ; y<bin_size/2; y++ ) {
yb0=-1; GHinit;
if(hasLf) { H0[O0[y]+1]+=ms[1]*M0[y]; H0[O1[y]+1]+=ms[1]*M1[y]; }
if(hasRt) { H0[O0[y]+hb+1]+=ms[3]*M0[y]; H0[O1[y]+hb+1]+=ms[3]*M1[y]; }
}
// main rows, has top and bottom bins, use SSE for minor speedup
for( ; ; y++ ) {
yb0 = (int) yb; if(yb0>=hb-1) break; GHinit;
_m0=pcl::sse_set(M0[y]); _m1=pcl::sse_set(M1[y]);
if(hasLf) { _m=pcl::sse_set(0,0,ms[1],ms[0]);
GH(H0+O0[y],_m,_m0); GH(H0+O1[y],_m,_m1); }
if(hasRt) { _m=pcl::sse_set(0,0,ms[3],ms[2]);
GH(H0+O0[y]+hb,_m,_m0); GH(H0+O1[y]+hb,_m,_m1); }
}
// final rows, no bottom bin_size
for( ; y<h0; y++ ) {
yb0 = (int) yb; GHinit;
if(hasLf) { H0[O0[y]]+=ms[0]*M0[y]; H0[O1[y]]+=ms[0]*M1[y]; }
if(hasRt) { H0[O0[y]+hb]+=ms[2]*M0[y]; H0[O1[y]+hb]+=ms[2]*M1[y]; }
}
#undef GHinit
#undef GH
#else
float ms[4], xyd, yb, xd, yd;
bool hasLf, hasRt; int xb0, yb0;
if( x==0 ) { init=(0+.5f)*sInv-0.5f; xb=init; }
hasLf = xb>=0; xb0 = hasLf?(int)xb:-1; hasRt = xb0 < wb-1;
xd=xb-xb0; xb+=sInv; yb=init; y=0;
// macros for code conciseness
#define GHinit yd=yb-yb0; yb+=sInv; H0=H+xb0*hb+yb0; xyd=xd*yd; \
ms[0]=1-xd-yd+xyd; ms[1]=yd-xyd; ms[2]=xd-xyd; ms[3]=xyd;
// leading rows, no top bin_size
for( ; y<bin_size/2; y++ ) {
yb0=-1; GHinit;
if(hasLf) { H0[O0[y]+1]+=ms[1]*M0[y]; H0[O1[y]+1]+=ms[1]*M1[y]; }
if(hasRt) { H0[O0[y]+hb+1]+=ms[3]*M0[y]; H0[O1[y]+hb+1]+=ms[3]*M1[y]; }
}
// main rows, has top and bottom bins
for( ; ; y++ ) {
yb0 = (int) yb;
if(yb0>=hb-1)
break;
GHinit;
if(hasLf)
{
H0[O0[y]+1]+=ms[1]*M0[y];
H0[O1[y]+1]+=ms[1]*M1[y];
H0[O0[y]]+=ms[0]*M0[y];
H0[O1[y]]+=ms[0]*M1[y];
}
if(hasRt)
{
H0[O0[y]+hb+1]+=ms[3]*M0[y];
H0[O1[y]+hb+1]+=ms[3]*M1[y];
H0[O0[y]+hb]+=ms[2]*M0[y];
H0[O1[y]+hb]+=ms[2]*M1[y];
}
}
// final rows, no bottom bin_size
for( ; y<h0; y++ ) {
yb0 = (int) yb; GHinit;
if(hasLf) { H0[O0[y]]+=ms[0]*M0[y]; H0[O1[y]]+=ms[0]*M1[y]; }
if(hasRt) { H0[O0[y]+hb]+=ms[2]*M0[y]; H0[O1[y]+hb]+=ms[2]*M1[y]; }
}
#undef GHinit
#endif
}
}
alFree(O0); alFree(O1); alFree(M0); alFree(M1);
}
