CVD 0.8
cvd/tensor_voting.h
00001 #ifndef CVD_INC_TENSOR_VOTE_H
00002 #define CVD_INC_TENSOR_VOTE_H
00003 
00004 #include <cvd/image.h>
00005 #include <TooN/TooN.h>
00006 #include <TooN/helpers.h>
00007 #include <vector>
00008 #include <utility>
00009 
00010 namespace CVD
00011 {
00012     
00013     #ifndef DOXYGEN_IGNORE_INTERNAL
00014     namespace TensorVoting
00015     {
00016         struct TV_coord
00017         {
00018             std::ptrdiff_t o;
00019             int x;
00020             int y;
00021         };
00022 
00023         std::vector<std::pair<TV_coord, TooN::Matrix<2> > > compute_a_tensor_kernel(int radius, double cutoff, double angle, double sigma, double ratio, int row_stride);
00024         inline unsigned int quantize_half_angle(double r, int num_divs)
00025         {
00026             return  ((int)floor((r/M_PI+100) * num_divs + 0.5)) % num_divs;
00027         }
00028     }
00029 
00030     #endif
00031 
00080     template<class C> Image<TooN::Matrix<2> > dense_tensor_vote_gradients(const SubImage<C>& image, double sigma, double ratio, double cutoff=0.001, unsigned int num_divs = 4096)
00081     {
00082         using TooN::Matrix;
00083         using std::pair;
00084         using std::vector;
00085         using TensorVoting::TV_coord;
00086 
00087         Matrix<2> zero(TooN::Zeros);
00088         Image<Matrix<2> > field(image.size(), zero);
00089 
00090 
00091         //Kernel values go as exp(-x*x / sigma * sigma)
00092         //So, for cutoff = exp(-x*x / sigma * sigma)
00093         //ln cutoff = -x*x / sigma*sigma
00094         //x = sigma * sqrt(-ln cutoff)
00095         int kernel_radius =  (int)ceil(sigma * sqrt(-log(cutoff)));
00096 
00097 
00098         //First, build up the kernels
00099         vector<vector<pair<TV_coord, Matrix<2> > > > kernels;
00100         for(unsigned int i=0; i < num_divs; i++)
00101         {
00102             double angle =  M_PI * i / num_divs;
00103             kernels.push_back(TensorVoting::compute_a_tensor_kernel(kernel_radius, cutoff, angle, sigma, ratio, field.row_stride()));
00104         }
00105         
00106         
00107         for(int y= kernel_radius; y < field.size().y - kernel_radius; y++)
00108             for(int x= kernel_radius; x < field.size().x - kernel_radius; x++)
00109             {
00110                 double gx = ((double)image[y][x+1] - image[y][x-1])/2.;
00111                 double gy = ((double)image[y+1][x] - image[y-1][x])/2.;
00112 
00113                 double scale = sqrt(gx*gx + gy*gy);
00114                 unsigned int direction = TensorVoting::quantize_half_angle(M_PI/2 + atan2(gy,gx), num_divs);
00115 
00116                 const vector<pair<TV_coord, Matrix<2> > >& kernel = kernels[direction];
00117 
00118                 Matrix<2>* p = &field[y][x];
00119                 
00120                 //The matrices are all symmetric, so only use the upper right triangle.
00121                 for(unsigned int i=0; i < kernel.size(); i++)
00122                 {
00123                     p[kernel[i].first.o][0][0] += kernel[i].second[0][0] * scale;
00124                     p[kernel[i].first.o][0][1] += kernel[i].second[0][1] * scale;
00125                     p[kernel[i].first.o][1][1] += kernel[i].second[1][1] * scale;
00126                 }
00127             }
00128 
00129         //Now do the edges
00130         for(int y= 1; y < field.size().y-1; y++)
00131         {
00132             for(int x= 1; x < field.size().x-1; x++)
00133             {
00134                 //Skip the middle bit
00135                 if(y >= kernel_radius && y < field.size().y - kernel_radius && x == kernel_radius)
00136                     x = field.size().x - kernel_radius;
00137 
00138                 double gx = ((double)image[y][x+1] - image[y][x-1])/2.;
00139                 double gy = ((double)image[y+1][x] - image[y-1][x])/2.;
00140 
00141                 double scale = sqrt(gx*gx + gy*gy);
00142                 unsigned int direction = TensorVoting::quantize_half_angle(M_PI/2 + atan(gy / gx), num_divs);
00143 
00144                 const vector<pair<TV_coord, Matrix<2> > >& kernel = kernels[direction];
00145 
00146                 Matrix<2>* p = &field[y][x];
00147                 
00148                 //The matrices are all symmetric, so only use the upper right triangle.
00149                 for(unsigned int i=0; i < kernel.size(); i++)
00150                 {
00151                     if(kernel[i].first.y+y >= 0 && kernel[i].first.y+y < field.size().y && kernel[i].first.x+x >= 0 && kernel[i].first.x+x < field.size().x)
00152                     {
00153                         p[kernel[i].first.o][0][0] += kernel[i].second[0][0] * scale;
00154                         p[kernel[i].first.o][0][1] += kernel[i].second[0][1] * scale;
00155                         p[kernel[i].first.o][1][1] += kernel[i].second[1][1] * scale;
00156                     }
00157                 }
00158             }
00159         }
00160 
00161         //Copy over bits to make the matrices symmetric
00162         for(Image<Matrix<2> >:: iterator i=field.begin(); i != field.end(); i++)
00163             (*i)[1][0] = (*i)[0][1];
00164 
00165         return field;
00166     }
00167     
00168 
00169     #ifdef CVD_EXPERIMENTAL
00170 
00171     template<class C> Image<TooN::Matrix<2> > dense_tensor_vote_gradients_fast(const SubImage<C>& image, double sigma, double ratio, double cutoff=0.001, int num_divs = 4096)
00172     {
00173         using TooN::Matrix;
00174         using std::pair;
00175         using std::make_pair;
00176         using std::vector;
00177 
00178         Matrix<2> zero(TooN::Zeros);
00179         Image<Matrix<2> > ffield(image.size(), zero);
00180         Image<__m128> field(image.size());
00181         field.zero();
00182 
00183         
00184         //In much the same way as dense_tensor_vote_gradients, build up the kernel.
00185         int kernel_radius =  (int)ceil(sigma * sqrt(-log(cutoff)));
00186         vector<vector<pair<int, Matrix<2> > > > matrix_kernels;
00187         for(unsigned int i=0; i < num_divs; i++)
00188         {
00189             double angle =  M_PI * i / num_divs;
00190             matrix_kernels.push_back(TensorVoting::compute_a_tensor_kernel(kernel_radius, cutoff, angle, sigma, ratio, field.row_stride()));
00191         }
00192 
00193 
00194         //Put the kernel in aligned SSE registers.
00195         //Image<__m128> is used since it guarantees SSE aligned memory.
00196         vector<vector<int> > kernel_offsets;
00197         vector<Image<__m128> > kernel_values;
00198         for(unsigned int i=0; i < matrix_kernels.size(); i++)
00199         {
00200             vector<int>    off(matrix_kernels[i].size());
00201             Image<__m128>  val(ImageRef(matrix_kernels[i].size(), 1));
00202 
00203             for(unsigned int j=0; j < matrix_kernels[i].size(); j++)
00204             {
00205                 off[j] = matrix_kernels[i][j].first;
00206                 Matrix<2>& m = matrix_kernels[i][j].second;
00207                 val.data()[j] = _mm_setr_ps(m[0][0], m[0][1], m[1][0], m[1][1]);
00208             }
00209 
00210             kernel_offsets.push_back(off);
00211             kernel_values.push_back(val);
00212         }
00213 
00214         for(int y= kernel_radius; y < field.size().y - kernel_radius; y++)
00215             for(int x= kernel_radius; x < field.size().x - kernel_radius; x++)
00216             {
00217                 float gx = ((float)image[y][x+1] - image[y][x-1])/2.;
00218                 float gy = ((float)image[y+1][x] - image[y-1][x])/2.;
00219 
00220                 float scale = sqrt(gx*gx + gy*gy);
00221                 unsigned int direction = TensorVoting::quantize_half_angle(M_PI/2 + atan(gy / gx), num_divs);
00222 
00223                 const vector<int> & off = kernel_offsets[direction];
00224                 __m128* val = kernel_values[direction].data();
00225                 __m128* p = &field[y][x];
00226                 __m128 s = _mm_set1_ps(scale);
00227 
00228                 for(unsigned int i=0; i < off.size(); i++)
00229                     p[off[i]] = _mm_add_ps(p[off[i]], _mm_mul_ps(val[i], s));
00230             }
00231 
00232         for(int y=0; y < field.size().y; y++)
00233             for(int x=0; x < field.size().x; x++)
00234             {
00235                 float f[4];
00236                 _mm_storeu_ps(f, field[y][x]);
00237                 ffield[y][x][0][0] = f[0];
00238                 ffield[y][x][0][1] = f[1];
00239                 ffield[y][x][1][0] = f[2];
00240                 ffield[y][x][1][1] = f[3];
00241             }
00242         
00243         return ffield;
00244     }
00245     #endif
00246 
00247 }
00248 
00249 
00250 #endif