src/piotr_fhog/gradientMex.cpp

   1 /*******************************************************************************
   2 * Piotr's Computer Vision Matlab Toolbox      Version 3.30
   3 * Copyright 2014 Piotr Dollar & Ron Appel.  [pdollar-at-gmail.com]
   4 * Licensed under the Simplified BSD License [see external/bsd.txt]
   5 *******************************************************************************/
   6
   7 #include "wrappers.hpp"
   8 #include <math.h>
   9 #include "string.h"
  10
  11 #include "sse.hpp"
  12
  13 #define PI 3.14159265f
  14
  15 // compute x and y gradients for just one column (uses sse)
  16 void grad1( float *I, float *Gx, float *Gy, int h, int w, int x ) {
  17   int y, y1; float *Ip, *In, r; __m128 *_Ip, *_In, *_G, _r;
  18   // compute column of Gx
  19   Ip=I-h; In=I+h; r=.5f;
  20   if(x==0) { r=1; Ip+=h; } else if(x==w-1) { r=1; In-=h; }
  21   if( h<4 || h%4>0 || (size_t(I)&15) || (size_t(Gx)&15) ) {
  22     for( y=0; y<h; y++ ) *Gx++=(*In++-*Ip++)*r;
  23   } else {
  24     _G=(__m128*) Gx; _Ip=(__m128*) Ip; _In=(__m128*) In; _r = SET(r);
  25     for(y=0; y<h; y+=4) *_G++=MUL(SUB(*_In++,*_Ip++),_r);
  26   }
  27   // compute column of Gy
  28   #define GRADY(r) *Gy++=(*In++-*Ip++)*r;
  29   Ip=I; In=Ip+1;
  30   // GRADY(1); Ip--; for(y=1; y<h-1; y++) GRADY(.5f); In--; GRADY(1);
  31   y1=((~((size_t) Gy) + 1) & 15)/4; if(y1==0) y1=4; if(y1>h-1) y1=h-1;
  32   GRADY(1); Ip--; for(y=1; y<y1; y++) GRADY(.5f);
  33   _r = SET(.5f); _G=(__m128*) Gy;
  34   for(; y+4<h-1; y+=4, Ip+=4, In+=4, Gy+=4)
  35     *_G++=MUL(SUB(LDu(*In),LDu(*Ip)),_r);
  36   for(; y<h-1; y++) GRADY(.5f); In--; GRADY(1);
  37   #undef GRADY
  38 }
  39
  40 // compute x and y gradients at each location (uses sse)
  41 void grad2( float *I, float *Gx, float *Gy, int h, int w, int d ) {
  42   int o, x, c, a=w*h; for(c=0; c<d; c++) for(x=0; x<w; x++) {
  43     o=c*a+x*h; grad1( I+o, Gx+o, Gy+o, h, w, x );
  44   }
  45 }
  46
  47 // build lookup table a[] s.t. a[x*n]~=acos(x) for x in [-1,1]
  48 float* acosTable() {
  49   const int n=10000, b=10; int i;
  50   static float a[n*2+b*2]; static bool init=false;
  51   float *a1=a+n+b; if( init ) return a1;
  52   for( i=-n-b; i<-n; i++ )   a1[i]=PI;
  53   for( i=-n; i<n; i++ )      a1[i]=float(acos(i/float(n)));
  54   for( i=n; i<n+b; i++ )     a1[i]=0;
  55   for( i=-n-b; i<n/10; i++ ) if( a1[i] > PI-1e-6f ) a1[i]=PI-1e-6f;
  56   init=true; return a1;
  57 }
  58
  59 // compute gradient magnitude and orientation at each location (uses sse)
  60 void gradMag( float *I, float *M, float *O, int h, int w, int d, bool full ) {
  61   int x, y, y1, c, h4, s; float *Gx, *Gy, *M2; __m128 *_Gx, *_Gy, *_M2, _m;
  62   float *acost = acosTable(), acMult=10000.0f;
  63   // allocate memory for storing one column of output (padded so h4%4==0)
  64   h4=(h%4==0) ? h : h-(h%4)+4; s=d*h4*sizeof(float);
  65   M2=(float*) alMalloc(s,16); _M2=(__m128*) M2;
  66   Gx=(float*) alMalloc(s,16); _Gx=(__m128*) Gx;
  67   Gy=(float*) alMalloc(s,16); _Gy=(__m128*) Gy;
  68   // compute gradient magnitude and orientation for each column
  69   for( x=0; x<w; x++ ) {
  70     // compute gradients (Gx, Gy) with maximum squared magnitude (M2)
  71     for(c=0; c<d; c++) {
  72       grad1( I+x*h+c*w*h, Gx+c*h4, Gy+c*h4, h, w, x );
  73       for( y=0; y<h4/4; y++ ) {
  74         y1=h4/4*c+y;
  75         _M2[y1]=ADD(MUL(_Gx[y1],_Gx[y1]),MUL(_Gy[y1],_Gy[y1]));
  76         if( c==0 ) continue;
  77         _m = CMPGT( _M2[y1], _M2[y] );
  78         _M2[y] = OR( AND(_m,_M2[y1]), ANDNOT(_m,_M2[y]) );
  79         _Gx[y] = OR( AND(_m,_Gx[y1]), ANDNOT(_m,_Gx[y]) );
  80         _Gy[y] = OR( AND(_m,_Gy[y1]), ANDNOT(_m,_Gy[y]) );
  81       }
  82     }
  83     // compute gradient mangitude (M) and normalize Gx
  84     for( y=0; y<h4/4; y++ ) {
  85       _m = MIN( RCPSQRT(_M2[y]), SET(1e10f) );
  86       _M2[y] = RCP(_m);
  87       if(O) _Gx[y] = MUL( MUL(_Gx[y],_m), SET(acMult) );
  88       if(O) _Gx[y] = XOR( _Gx[y], AND(_Gy[y], SET(-0.f)) );
  89     };
  90     memcpy( M+x*h, M2, h*sizeof(float) );
  91     // compute and store gradient orientation (O) via table lookup
  92     if( O!=0 ) for( y=0; y<h; y++ ) O[x*h+y] = acost[(int)Gx[y]];
  93     if( O!=0 && full ) {
  94       y1=((~size_t(O+x*h)+1)&15)/4; y=0;
  95       for( ; y<y1; y++ ) O[y+x*h]+=(Gy[y]<0)*PI;
  96       for( ; y<h-4; y+=4 ) STRu( O[y+x*h],
  97         ADD( LDu(O[y+x*h]), AND(CMPLT(LDu(Gy[y]),SET(0.f)),SET(PI)) ) );
  98       for( ; y<h; y++ ) O[y+x*h]+=(Gy[y]<0)*PI;
  99     }
 100   }
 101   alFree(Gx); alFree(Gy); alFree(M2);
 102 }
 103
 104 // normalize gradient magnitude at each location (uses sse)
 105 void gradMagNorm( float *M, float *S, int h, int w, float norm ) {
 106   __m128 *_M, *_S, _norm; int i=0, n=h*w, n4=n/4;
 107   _S = (__m128*) S; _M = (__m128*) M; _norm = SET(norm);
 108   bool sse = !(size_t(M)&15) && !(size_t(S)&15);
 109   if(sse) {
 110       for(; i<n4; i++) { *_M=MUL(*_M,RCP(ADD(*_S++,_norm))); _M++; }
 111       i*=4;
 112   }
 113   for(; i<n; i++) M[i] /= (S[i] + norm);
 114 }
 115
 116 // helper for gradHist, quantize O and M into O0, O1 and M0, M1 (uses sse)
 117 void gradQuantize( float *O, float *M, int *O0, int *O1, float *M0, float *M1,
 118   int nb, int n, float norm, int nOrients, bool full, bool interpolate )
 119 {
 120   // assumes all *OUTPUT* matrices are 4-byte aligned
 121   int i, o0, o1; float o, od, m;
 122   __m128i _o0, _o1, *_O0, *_O1; __m128 _o, _od, _m, *_M0, *_M1;
 123   // define useful constants
 124   const float oMult=(float)nOrients/(full?2*PI:PI); const int oMax=nOrients*nb;
 125   const __m128 _norm=SET(norm), _oMult=SET(oMult), _nbf=SET((float)nb);
 126   const __m128i _oMax=SET(oMax), _nb=SET(nb);
 127   // perform the majority of the work with sse
 128   _O0=(__m128i*) O0; _O1=(__m128i*) O1; _M0=(__m128*) M0; _M1=(__m128*) M1;
 129   if( interpolate ) for( i=0; i<=n-4; i+=4 ) {
 130     _o=MUL(LDu(O[i]),_oMult); _o0=CVT(_o); _od=SUB(_o,CVT(_o0));
 131     _o0=CVT(MUL(CVT(_o0),_nbf)); _o0=AND(CMPGT(_oMax,_o0),_o0); *_O0++=_o0;
 132     _o1=ADD(_o0,_nb); _o1=AND(CMPGT(_oMax,_o1),_o1); *_O1++=_o1;
 133     _m=MUL(LDu(M[i]),_norm); *_M1=MUL(_od,_m); *_M0++=SUB(_m,*_M1); _M1++;
 134   } else for( i=0; i<=n-4; i+=4 ) {
 135     _o=MUL(LDu(O[i]),_oMult); _o0=CVT(ADD(_o,SET(.5f)));
 136     _o0=CVT(MUL(CVT(_o0),_nbf)); _o0=AND(CMPGT(_oMax,_o0),_o0); *_O0++=_o0;
 137     *_M0++=MUL(LDu(M[i]),_norm); *_M1++=SET(0.f); *_O1++=SET(0);
 138   }
 139   // compute trailing locations without sse
 140   if( interpolate ) for(; i<n; i++ ) {
 141     o=O[i]*oMult; o0=(int) o; od=o-o0;
 142     o0*=nb; if(o0>=oMax) o0=0; O0[i]=o0;
 143     o1=o0+nb; if(o1==oMax) o1=0; O1[i]=o1;
 144     m=M[i]*norm; M1[i]=od*m; M0[i]=m-M1[i];
 145   } else for(; i<n; i++ ) {
 146     o=O[i]*oMult; o0=(int) (o+.5f);
 147     o0*=nb; if(o0>=oMax) o0=0; O0[i]=o0;
 148     M0[i]=M[i]*norm; M1[i]=0; O1[i]=0;
 149   }
 150 }
 151
 152 // compute nOrients gradient histograms per bin x bin block of pixels
 153 void gradHist( float *M, float *O, float *H, int h, int w,
 154   int bin, int nOrients, int softBin, bool full )
 155 {
 156   const int hb=h/bin, wb=w/bin, h0=hb*bin, w0=wb*bin, nb=wb*hb;
 157   const float s=(float)bin, sInv=1/s, sInv2=1/s/s;
 158   float *H0, *H1, *M0, *M1; int x, y; int *O0, *O1; float xb, init;
 159   O0=(int*)alMalloc(h*sizeof(int),16); M0=(float*) alMalloc(h*sizeof(float),16);
 160   O1=(int*)alMalloc(h*sizeof(int),16); M1=(float*) alMalloc(h*sizeof(float),16);
 161   // main loop
 162   for( x=0; x<w0; x++ ) {
 163     // compute target orientation bins for entire column - very fast
 164     gradQuantize(O+x*h,M+x*h,O0,O1,M0,M1,nb,h0,sInv2,nOrients,full,softBin>=0);
 165
 166     if( softBin<0 && softBin%2==0 ) {
 167       // no interpolation w.r.t. either orienation or spatial bin
 168       H1=H+(x/bin)*hb;
 169       #define GH H1[O0[y]]+=M0[y]; y++;
 170       if( bin==1 )      for(y=0; y<h0;) { GH; H1++; }
 171       else if( bin==2 ) for(y=0; y<h0;) { GH; GH; H1++; }
 172       else if( bin==3 ) for(y=0; y<h0;) { GH; GH; GH; H1++; }
 173       else if( bin==4 ) for(y=0; y<h0;) { GH; GH; GH; GH; H1++; }
 174       else for( y=0; y<h0;) { for( int y1=0; y1<bin; y1++ ) { GH; } H1++; }
 175       #undef GH
 176
 177     } else if( softBin%2==0 || bin==1 ) {
 178       // interpolate w.r.t. orientation only, not spatial bin
 179       H1=H+(x/bin)*hb;
 180       #define GH H1[O0[y]]+=M0[y]; H1[O1[y]]+=M1[y]; y++;
 181       if( bin==1 )      for(y=0; y<h0;) { GH; H1++; }
 182       else if( bin==2 ) for(y=0; y<h0;) { GH; GH; H1++; }
 183       else if( bin==3 ) for(y=0; y<h0;) { GH; GH; GH; H1++; }
 184       else if( bin==4 ) for(y=0; y<h0;) { GH; GH; GH; GH; H1++; }
 185       else for( y=0; y<h0;) { for( int y1=0; y1<bin; y1++ ) { GH; } H1++; }
 186       #undef GH
 187
 188     } else {
 189       // interpolate using trilinear interpolation
 190       float ms[4], xyd, yb, xd, yd; __m128 _m, _m0, _m1;
 191       bool hasLf, hasRt; int xb0, yb0;
 192       if( x==0 ) { init=(0+.5f)*sInv-0.5f; xb=init; }
 193       hasLf = xb>=0; xb0 = hasLf?(int)xb:-1; hasRt = xb0 < wb-1;
 194       xd=xb-xb0; xb+=sInv; yb=init; y=0;
 195       // macros for code conciseness
 196       #define GHinit yd=yb-yb0; yb+=sInv; H0=H+xb0*hb+yb0; xyd=xd*yd; \
 197         ms[0]=1-xd-yd+xyd; ms[1]=yd-xyd; ms[2]=xd-xyd; ms[3]=xyd;
 198       #define GH(H,ma,mb) H1=H; STRu(*H1,ADD(LDu(*H1),MUL(ma,mb)));
 199       // leading rows, no top bin
 200       for( ; y<bin/2; y++ ) {
 201         yb0=-1; GHinit;
 202         if(hasLf) { H0[O0[y]+1]+=ms[1]*M0[y]; H0[O1[y]+1]+=ms[1]*M1[y]; }
 203         if(hasRt) { H0[O0[y]+hb+1]+=ms[3]*M0[y]; H0[O1[y]+hb+1]+=ms[3]*M1[y]; }
 204       }
 205       // main rows, has top and bottom bins, use SSE for minor speedup
 206       if( softBin<0 ) for( ; ; y++ ) {
 207         yb0 = (int) yb; if(yb0>=hb-1) break; GHinit; _m0=SET(M0[y]);
 208         if(hasLf) { _m=SET(0,0,ms[1],ms[0]); GH(H0+O0[y],_m,_m0); }
 209         if(hasRt) { _m=SET(0,0,ms[3],ms[2]); GH(H0+O0[y]+hb,_m,_m0); }
 210       } else for( ; ; y++ ) {
 211         yb0 = (int) yb; if(yb0>=hb-1) break; GHinit;
 212         _m0=SET(M0[y]); _m1=SET(M1[y]);
 213         if(hasLf) { _m=SET(0,0,ms[1],ms[0]);
 214           GH(H0+O0[y],_m,_m0); GH(H0+O1[y],_m,_m1); }
 215         if(hasRt) { _m=SET(0,0,ms[3],ms[2]);
 216           GH(H0+O0[y]+hb,_m,_m0); GH(H0+O1[y]+hb,_m,_m1); }
 217       }
 218       // final rows, no bottom bin
 219       for( ; y<h0; y++ ) {
 220         yb0 = (int) yb; GHinit;
 221         if(hasLf) { H0[O0[y]]+=ms[0]*M0[y]; H0[O1[y]]+=ms[0]*M1[y]; }
 222         if(hasRt) { H0[O0[y]+hb]+=ms[2]*M0[y]; H0[O1[y]+hb]+=ms[2]*M1[y]; }
 223       }
 224       #undef GHinit
 225       #undef GH
 226     }
 227   }
 228   alFree(O0); alFree(O1); alFree(M0); alFree(M1);
 229   // normalize boundary bins which only get 7/8 of weight of interior bins
 230   if( softBin%2!=0 ) for( int o=0; o<nOrients; o++ ) {
 231     x=0; for( y=0; y<hb; y++ ) H[o*nb+x*hb+y]*=8.f/7.f;
 232     y=0; for( x=0; x<wb; x++ ) H[o*nb+x*hb+y]*=8.f/7.f;
 233     x=wb-1; for( y=0; y<hb; y++ ) H[o*nb+x*hb+y]*=8.f/7.f;
 234     y=hb-1; for( x=0; x<wb; x++ ) H[o*nb+x*hb+y]*=8.f/7.f;
 235   }
 236 }
 237
 238 /******************************************************************************/
 239
 240 // HOG helper: compute 2x2 block normalization values (padded by 1 pixel)
 241 float* hogNormMatrix( float *H, int nOrients, int hb, int wb, int bin ) {
 242   float *N, *N1, *n; int o, x, y, dx, dy, hb1=hb+1, wb1=wb+1;
 243   float eps = 1e-4f/4/bin/bin/bin/bin; // precise backward equality
 244   N = (float*) wrCalloc(hb1*wb1,sizeof(float)); N1=N+hb1+1;
 245   for( o=0; o<nOrients; o++ ) for( x=0; x<wb; x++ ) for( y=0; y<hb; y++ )
 246     N1[x*hb1+y] += H[o*wb*hb+x*hb+y]*H[o*wb*hb+x*hb+y];
 247   for( x=0; x<wb-1; x++ ) for( y=0; y<hb-1; y++ ) {
 248     n=N1+x*hb1+y; *n=1/float(sqrt(n[0]+n[1]+n[hb1]+n[hb1+1]+eps)); }
 249   x=0;     dx= 1; dy= 1; y=0;                  N[x*hb1+y]=N[(x+dx)*hb1+y+dy];
 250   x=0;     dx= 1; dy= 0; for(y=0; y<hb1; y++)  N[x*hb1+y]=N[(x+dx)*hb1+y+dy];
 251   x=0;     dx= 1; dy=-1; y=hb1-1;              N[x*hb1+y]=N[(x+dx)*hb1+y+dy];
 252   x=wb1-1; dx=-1; dy= 1; y=0;                  N[x*hb1+y]=N[(x+dx)*hb1+y+dy];
 253   x=wb1-1; dx=-1; dy= 0; for( y=0; y<hb1; y++) N[x*hb1+y]=N[(x+dx)*hb1+y+dy];
 254   x=wb1-1; dx=-1; dy=-1; y=hb1-1;              N[x*hb1+y]=N[(x+dx)*hb1+y+dy];
 255   y=0;     dx= 0; dy= 1; for(x=0; x<wb1; x++)  N[x*hb1+y]=N[(x+dx)*hb1+y+dy];
 256   y=hb1-1; dx= 0; dy=-1; for(x=0; x<wb1; x++)  N[x*hb1+y]=N[(x+dx)*hb1+y+dy];
 257   return N;
 258 }
 259
 260 // HOG helper: compute HOG or FHOG channels
 261 void hogChannels( float *H, const float *R, const float *N,
 262   int hb, int wb, int nOrients, float clip, int type )
 263 {
 264   #define GETT(blk) t=R1[y]*N1[y-(blk)]; if(t>clip) t=clip; c++;
 265   const float r=.2357f; int o, x, y, c; float t;
 266   const int nb=wb*hb, nbo=nOrients*nb, hb1=hb+1;
 267   for( o=0; o<nOrients; o++ ) for( x=0; x<wb; x++ ) {
 268     const float *R1=R+o*nb+x*hb, *N1=N+x*hb1+hb1+1;
 269     float *H1 = (type<=1) ? (H+o*nb+x*hb) : (H+x*hb);
 270     if( type==0) for( y=0; y<hb; y++ ) {
 271       // store each orientation and normalization (nOrients*4 channels)
 272       c=-1; GETT(0); H1[c*nbo+y]=t; GETT(1); H1[c*nbo+y]=t;
 273       GETT(hb1); H1[c*nbo+y]=t; GETT(hb1+1); H1[c*nbo+y]=t;
 274     } else if( type==1 ) for( y=0; y<hb; y++ ) {
 275       // sum across all normalizations (nOrients channels)
 276       c=-1; GETT(0); H1[y]+=t*.5f; GETT(1); H1[y]+=t*.5f;
 277       GETT(hb1); H1[y]+=t*.5f; GETT(hb1+1); H1[y]+=t*.5f;
 278     } else if( type==2 ) for( y=0; y<hb; y++ ) {
 279       // sum across all orientations (4 channels)
 280       c=-1; GETT(0); H1[c*nb+y]+=t*r; GETT(1); H1[c*nb+y]+=t*r;
 281       GETT(hb1); H1[c*nb+y]+=t*r; GETT(hb1+1); H1[c*nb+y]+=t*r;
 282     }
 283   }
 284   #undef GETT
 285 }
 286
 287 // compute HOG features
 288 void hog( float *M, float *O, float *H, int h, int w, int binSize,
 289   int nOrients, int softBin, bool full, float clip )
 290 {
 291   float *N, *R; const int hb=h/binSize, wb=w/binSize, nb=hb*wb;
 292   (void) nb;
 293   // compute unnormalized gradient histograms
 294   R = (float*) wrCalloc(wb*hb*nOrients,sizeof(float));
 295   gradHist( M, O, R, h, w, binSize, nOrients, softBin, full );
 296   // compute block normalization values
 297   N = hogNormMatrix( R, nOrients, hb, wb, binSize );
 298   // perform four normalizations per spatial block
 299   hogChannels( H, R, N, hb, wb, nOrients, clip, 0 );
 300   wrFree(N); wrFree(R);
 301 }
 302
 303 // compute FHOG features
 304 void fhog( float *M, float *O, float *H, int h, int w, int binSize,
 305   int nOrients, int softBin, float clip )
 306 {
 307   const int hb=h/binSize, wb=w/binSize, nb=hb*wb, nbo=nb*nOrients;
 308   float *N, *R1, *R2; int o, x;
 309   // compute unnormalized constrast sensitive histograms
 310   R1 = (float*) wrCalloc(wb*hb*nOrients*2 + 2,sizeof(float));
 311   gradHist( M, O, R1, h, w, binSize, nOrients*2, softBin, true );
 312   // compute unnormalized contrast insensitive histograms
 313   R2 = (float*) wrCalloc(wb*hb*nOrients,sizeof(float));
 314   for( o=0; o<nOrients; o++ ) for( x=0; x<nb; x++ )
 315     R2[o*nb+x] = R1[o*nb+x]+R1[(o+nOrients)*nb+x];
 316   // compute block normalization values
 317   N = hogNormMatrix( R2, nOrients, hb, wb, binSize );
 318   // normalized histograms and texture channels
 319   hogChannels( H+nbo*0, R1, N, hb, wb, nOrients*2, clip, 1 );
 320   hogChannels( H+nbo*2, R2, N, hb, wb, nOrients*1, clip, 1 );
 321   hogChannels( H+nbo*3, R1, N, hb, wb, nOrients*2, clip, 2 );
 322   wrFree(N); wrFree(R1); wrFree(R2);
 323 }
 324
 325 /******************************************************************************/
 326 #ifdef MATLAB_MEX_FILE
 327 // Create [hxwxd] mxArray array, initialize to 0 if c=true
 328 mxArray* mxCreateMatrix3( int h, int w, int d, mxClassID id, bool c, void **I ){
 329   const int dims[3]={h,w,d}, n=h*w*d; int b; mxArray* M;
 330   if( id==mxINT32_CLASS ) b=sizeof(int);
 331   else if( id==mxDOUBLE_CLASS ) b=sizeof(double);
 332   else if( id==mxSINGLE_CLASS ) b=sizeof(float);
 333   else mexErrMsgTxt("Unknown mxClassID.");
 334   *I = c ? mxCalloc(n,b) : mxMalloc(n*b);
 335   M = mxCreateNumericMatrix(0,0,id,mxREAL);
 336   mxSetData(M,*I); mxSetDimensions(M,dims,3); return M;
 337 }
 338
 339 // Check inputs and outputs to mex, retrieve first input I
 340 void checkArgs( int nl, mxArray *pl[], int nr, const mxArray *pr[], int nl0,
 341   int nl1, int nr0, int nr1, int *h, int *w, int *d, mxClassID id, void **I )
 342 {
 343   const int *dims; int nDims;
 344   if( nl<nl0 || nl>nl1 ) mexErrMsgTxt("Incorrect number of outputs.");
 345   if( nr<nr0 || nr>nr1 ) mexErrMsgTxt("Incorrect number of inputs.");
 346   nDims = mxGetNumberOfDimensions(pr[0]); dims = mxGetDimensions(pr[0]);
 347   *h=dims[0]; *w=dims[1]; *d=(nDims==2) ? 1 : dims[2]; *I = mxGetPr(pr[0]);
 348   if( nDims!=2 && nDims!=3 ) mexErrMsgTxt("I must be a 2D or 3D array.");
 349   if( mxGetClassID(pr[0])!=id ) mexErrMsgTxt("I has incorrect type.");
 350 }
 351
 352 // [Gx,Gy] = grad2(I) - see gradient2.m
 353 void mGrad2( int nl, mxArray *pl[], int nr, const mxArray *pr[] ) {
 354   int h, w, d; float *I, *Gx, *Gy;
 355   checkArgs(nl,pl,nr,pr,1,2,1,1,&h,&w,&d,mxSINGLE_CLASS,(void**)&I);
 356   if(h<2 || w<2) mexErrMsgTxt("I must be at least 2x2.");
 357   pl[0]= mxCreateMatrix3( h, w, d, mxSINGLE_CLASS, 0, (void**) &Gx );
 358   pl[1]= mxCreateMatrix3( h, w, d, mxSINGLE_CLASS, 0, (void**) &Gy );
 359   grad2( I, Gx, Gy, h, w, d );
 360 }
 361
 362 // [M,O] = gradMag( I, channel, full ) - see gradientMag.m
 363 void mGradMag( int nl, mxArray *pl[], int nr, const mxArray *pr[] ) {
 364   int h, w, d, c, full; float *I, *M, *O=0;
 365   checkArgs(nl,pl,nr,pr,1,2,3,3,&h,&w,&d,mxSINGLE_CLASS,(void**)&I);
 366   if(h<2 || w<2) mexErrMsgTxt("I must be at least 2x2.");
 367   c = (int) mxGetScalar(pr[1]); full = (int) mxGetScalar(pr[2]);
 368   if( c>0 && c<=d ) { I += h*w*(c-1); d=1; }
 369   pl[0] = mxCreateMatrix3(h,w,1,mxSINGLE_CLASS,0,(void**)&M);
 370   if(nl>=2) pl[1] = mxCreateMatrix3(h,w,1,mxSINGLE_CLASS,0,(void**)&O);
 371   gradMag(I, M, O, h, w, d, full>0 );
 372 }
 373
 374 // gradMagNorm( M, S, norm ) - operates on M - see gradientMag.m
 375 void mGradMagNorm( int nl, mxArray *pl[], int nr, const mxArray *pr[] ) {
 376   int h, w, d; float *M, *S, norm;
 377   checkArgs(nl,pl,nr,pr,0,0,3,3,&h,&w,&d,mxSINGLE_CLASS,(void**)&M);
 378   if( mxGetM(pr[1])!=h || mxGetN(pr[1])!=w || d!=1 ||
 379     mxGetClassID(pr[1])!=mxSINGLE_CLASS ) mexErrMsgTxt("M or S is bad.");
 380   S = (float*) mxGetPr(pr[1]); norm = (float) mxGetScalar(pr[2]);
 381   gradMagNorm(M,S,h,w,norm);
 382 }
 383
 384 // H=gradHist(M,O,[...]) - see gradientHist.m
 385 void mGradHist( int nl, mxArray *pl[], int nr, const mxArray *pr[] ) {
 386   int h, w, d, hb, wb, nChns, binSize, nOrients, softBin, useHog;
 387   bool full; float *M, *O, *H, clipHog;
 388   checkArgs(nl,pl,nr,pr,1,3,2,8,&h,&w,&d,mxSINGLE_CLASS,(void**)&M);
 389   O = (float*) mxGetPr(pr[1]);
 390   if( mxGetM(pr[1])!=h || mxGetN(pr[1])!=w || d!=1 ||
 391     mxGetClassID(pr[1])!=mxSINGLE_CLASS ) mexErrMsgTxt("M or O is bad.");
 392   binSize  = (nr>=3) ? (int)   mxGetScalar(pr[2])    : 8;
 393   nOrients = (nr>=4) ? (int)   mxGetScalar(pr[3])    : 9;
 394   softBin  = (nr>=5) ? (int)   mxGetScalar(pr[4])    : 1;
 395   useHog   = (nr>=6) ? (int)   mxGetScalar(pr[5])    : 0;
 396   clipHog  = (nr>=7) ? (float) mxGetScalar(pr[6])    : 0.2f;
 397   full     = (nr>=8) ? (bool) (mxGetScalar(pr[7])>0) : false;
 398   hb = h/binSize; wb = w/binSize;
 399   nChns = useHog== 0 ? nOrients : (useHog==1 ? nOrients*4 : nOrients*3+5);
 400   pl[0] = mxCreateMatrix3(hb,wb,nChns,mxSINGLE_CLASS,1,(void**)&H);
 401   if( nOrients==0 ) return;
 402   if( useHog==0 ) {
 403     gradHist( M, O, H, h, w, binSize, nOrients, softBin, full );
 404   } else if(useHog==1) {
 405     hog( M, O, H, h, w, binSize, nOrients, softBin, full, clipHog );
 406   } else {
 407     fhog( M, O, H, h, w, binSize, nOrients, softBin, clipHog );
 408   }
 409 }
 410
 411 // inteface to various gradient functions (see corresponding Matlab functions)
 412 void mexFunction( int nl, mxArray *pl[], int nr, const mxArray *pr[] ) {
 413   int f; char action[1024]; f=mxGetString(pr[0],action,1024); nr--; pr++;
 414   if(f) mexErrMsgTxt("Failed to get action.");
 415   else if(!strcmp(action,"gradient2")) mGrad2(nl,pl,nr,pr);
 416   else if(!strcmp(action,"gradientMag")) mGradMag(nl,pl,nr,pr);
 417   else if(!strcmp(action,"gradientMagNorm")) mGradMagNorm(nl,pl,nr,pr);
 418   else if(!strcmp(action,"gradientHist")) mGradHist(nl,pl,nr,pr);
 419   else mexErrMsgTxt("Invalid action.");
 420 }
 421 #endif