CVD 0.8
|
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