| 139 | } |
| 140 | #ifdef SSE_PERMUTOHEDRAL |
| 141 | void Permutohedral::init ( const MatrixXf & feature ) |
| 142 | { |
| 143 | // Compute the lattice coordinates for each feature [there is going to be a lot of magic here |
| 144 | N_ = feature.cols(); |
| 145 | d_ = feature.rows(); |
| 146 | HashTable hash_table( d_, N_/**(d_+1)*/ ); |
| 147 | |
| 148 | const int blocksize = sizeof(__m128) / sizeof(float); |
| 149 | const __m128 invdplus1 = _mm_set1_ps( 1.0f / (d_+1) ); |
| 150 | const __m128 dplus1 = _mm_set1_ps( d_+1 ); |
| 151 | const __m128 Zero = _mm_set1_ps( 0 ); |
| 152 | const __m128 One = _mm_set1_ps( 1 ); |
| 153 | |
| 154 | // Allocate the class memory |
| 155 | offset_.resize( (d_+1)*(N_+16) ); |
| 156 | std::fill( offset_.begin(), offset_.end(), 0 ); |
| 157 | barycentric_.resize( (d_+1)*(N_+16) ); |
| 158 | std::fill( barycentric_.begin(), barycentric_.end(), 0 ); |
| 159 | rank_.resize( (d_+1)*(N_+16) ); |
| 160 | |
| 161 | // Allocate the local memory |
| 162 | __m128 * scale_factor = (__m128*) _mm_malloc( (d_ )*sizeof(__m128) , 16 ); |
| 163 | __m128 * f = (__m128*) _mm_malloc( (d_ )*sizeof(__m128) , 16 ); |
| 164 | __m128 * elevated = (__m128*) _mm_malloc( (d_+1)*sizeof(__m128) , 16 ); |
| 165 | __m128 * rem0 = (__m128*) _mm_malloc( (d_+1)*sizeof(__m128) , 16 ); |
| 166 | __m128 * rank = (__m128*) _mm_malloc( (d_+1)*sizeof(__m128), 16 ); |
| 167 | float * barycentric = new float[(d_+2)*blocksize]; |
| 168 | short * canonical = new short[(d_+1)*(d_+1)]; |
| 169 | short * key = new short[d_+1]; |
| 170 | |
| 171 | // Compute the canonical simplex |
| 172 | for( int i=0; i<=d_; i++ ){ |
| 173 | for( int j=0; j<=d_-i; j++ ) |
| 174 | canonical[i*(d_+1)+j] = i; |
| 175 | for( int j=d_-i+1; j<=d_; j++ ) |
| 176 | canonical[i*(d_+1)+j] = i - (d_+1); |
| 177 | } |
| 178 | |
| 179 | // Expected standard deviation of our filter (p.6 in [Adams etal 2010]) |
| 180 | float inv_std_dev = sqrt(2.0 / 3.0)*(d_+1); |
| 181 | // Compute the diagonal part of E (p.5 in [Adams etal 2010]) |
| 182 | for( int i=0; i<d_; i++ ) |
| 183 | scale_factor[i] = _mm_set1_ps( 1.0 / sqrt( (i+2)*(i+1) ) * inv_std_dev ); |
| 184 | |
| 185 | // Setup the SSE rounding |
| 186 | #ifndef __SSE4_1__ |
| 187 | const unsigned int old_rounding = _mm_getcsr(); |
| 188 | _mm_setcsr( (old_rounding&~_MM_ROUND_MASK) | _MM_ROUND_NEAREST ); |
| 189 | #endif |
| 190 | |
| 191 | // Compute the simplex each feature lies in |
| 192 | for( int k=0; k<N_; k+=blocksize ){ |
| 193 | // Load the feature from memory |
| 194 | float * ff = (float*)f; |
| 195 | for( int j=0; j<d_; j++ ) |
| 196 | for( int i=0; i<blocksize; i++ ) |
| 197 | ff[ j*blocksize + i ] = k+i < N_ ? feature(j,k+i) : 0.0; |
| 198 | |