00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #ifndef CVD_CONVOLUTION_H_
00023 #define CVD_CONVOLUTION_H_
00024
00025 #include <vector>
00026 #include <memory>
00027 #include <numeric>
00028 #include <algorithm>
00029
00030 #include <cvd/config.h>
00031 #include <cvd/exceptions.h>
00032 #include <cvd/image.h>
00033 #include <cvd/internal/pixel_operations.h>
00034 #include <cvd/internal/aligned_mem.h>
00035 #include <cvd/utility.h>
00036
00037 namespace CVD {
00038
00047 template <class T>
00048 T gaussianKernel(std::vector<T>& k, T maxval, double stddev)
00049 {
00050 double sum = 0;
00051 unsigned int i, argmax=0;
00052 std::vector<double> kernel(k.size());
00053 for (i=0;i<k.size();i++) {
00054 double x = i +0.5 - k.size()/2.0;
00055 sum += kernel[i] = exp(-x*x/(2*stddev*stddev));
00056 if (kernel[i] > kernel[argmax])
00057 argmax = i;
00058 }
00059 T finalSum = 0;
00060 for (i=0;i<k.size();i++)
00061 finalSum += k[i] = (T)(kernel[i]*maxval/kernel[argmax]);
00062 return finalSum;
00063 }
00064
00072 template <class S, class T>
00073 T scaleKernel(const std::vector<S>& k, std::vector<T>& scaled, T maxval)
00074 {
00075 unsigned int i,argmax=0;
00076 for (i=1;i<k.size();i++)
00077 if (k[i]>k[argmax])
00078 argmax = i;
00079 scaled.resize(k.size());
00080 T sum = 0;
00081 for (i=0;i<k.size();i++)
00082 sum += (scaled[i] = (T)((k[i]*maxval)/k[argmax]));
00083 return sum;
00084 }
00085
00086 template <class T>
00087 void convolveGaussian5_1(BasicImage<T>& I)
00088 {
00089 int w = I.size().x;
00090 int h = I.size().y;
00091 int i,j;
00092 for (j=0;j<w;j++) {
00093 T* src = I.data()+j;
00094 T* end = src + w*(h-4);
00095 while (src != end) {
00096 T sum= (T)(0.0544887*(src[0]+src[4*w])
00097 + 0.2442010*(src[w]+src[3*w])
00098 + 0.4026200*src[2*w]);
00099 *(src) = sum;
00100 src += w;
00101 }
00102 }
00103 for (i=h-5;i>=0;i--) {
00104 T* src = I.data()+i*w;
00105 T* end = src + w-4;
00106 while (src != end) {
00107 T sum= (T)(0.0544887*(src[0]+src[4])
00108 + 0.2442010*(src[1]+src[3])
00109 + 0.4026200*src[2]);
00110 *(src+2*w+2)=sum;
00111 ++src;
00112 }
00113 }
00114 }
00115
00116 namespace Exceptions {
00117
00120 namespace Convolution {
00123 struct All: public CVD::Exceptions::All {};
00124
00127 struct IncompatibleImageSizes : public All {
00128 IncompatibleImageSizes(const std::string & function)
00129 {
00130 what = "Incompatible image sizes in " + function;
00131 }
00132 };
00133
00134
00137 struct OddSizedKernelRequired : public All {
00138 OddSizedKernelRequired(const std::string & function)
00139 {
00140 what = "Odd sized kernel required in " + function;
00141 };
00142 };
00143 }
00144 }
00145
00146
00151 template <class T> void convolveWithBox(const BasicImage<T>& I, BasicImage<T>& J, ImageRef hwin)
00152 {
00153 typedef typename Pixel::traits<T>::wider_type sum_type;
00154 if (I.size() != J.size()) {
00155 throw Exceptions::Convolution::IncompatibleImageSizes("convolveWithBox");
00156 }
00157 int w = I.size().x;
00158 int h = I.size().y;
00159 ImageRef win = 2*hwin+ImageRef(1,1);
00160 const double factor = 1.0/(win.x*win.y);
00161 std::vector<sum_type> buffer(w*win.y);
00162 std::vector<sum_type> sums_v(w);
00163 sum_type* sums = &sums_v[0];
00164 sum_type* next_row = &buffer[0];
00165 sum_type* oldest_row = &buffer[0];
00166 zeroPixels(sums, w);
00167 const T* input = I.data();
00168 T* output = J[hwin.y] - hwin.x;
00169 for (int i=0; i<h; i++) {
00170 sum_type hsum=sum_type();
00171 const T* back = input;
00172 int j;
00173 for (j=0; j<win.x-1; j++)
00174 hsum += input[j];
00175 for (; j<w; j++) {
00176 hsum += input[j];
00177 next_row[j] = hsum;
00178 sums[j] += hsum;
00179 hsum -= *(back++);
00180 }
00181 if (i >= win.y-1) {
00182 assign_multiple(sums+win.x-1, factor, output+win.x-1, w-win.x+1);
00183 differences(sums+win.x-1, oldest_row+win.x-1, sums+win.x-1, w-win.x+1);
00184 output += w;
00185 oldest_row += w;
00186 if (oldest_row == &buffer[0] + w*win.y)
00187 oldest_row = &buffer[0];
00188 }
00189 input += w;
00190 next_row += w;
00191 if (next_row == &buffer[0] + w*win.y)
00192 next_row = &buffer[0];
00193 }
00194 }
00195
00196 template <class T> inline void convolveWithBox(const BasicImage<T>& I, BasicImage<T>& J, int hwin)
00197 {
00198 convolveWithBox(I, J, ImageRef(hwin,hwin));
00199 }
00200
00201 template <class T> inline void convolveWithBox(BasicImage<T>& I, int hwin) {
00202 convolveWithBox(I,I,hwin);
00203 }
00204
00205 template <class T> inline void convolveWithBox(BasicImage<T>& I, ImageRef hwin) {
00206 convolveWithBox(I,I,hwin);
00207 }
00208
00209
00210 template <class T, int A, int B, int C> void convolveSymmetric(Image<T>& I)
00211 {
00212 typedef typename Pixel::traits<T>::wider_type wider;
00213 static const wider S = (A+B+C+B+A);
00214 int width = I.size().x;
00215 int height = I.size().y;
00216 T* p = I.data();
00217 int i,j;
00218 for (i=0; i<height; i++) {
00219 wider a = p[0];
00220 wider b = p[1];
00221 wider c = p[2];
00222 wider d = p[3];
00223 p[0] = (T)(((c+c)*A+(b+b)*B + a*C) /S);
00224 p[1] = (T)(((b+d)*A+(a+c)*B + b*C) /S);
00225 for (j=0;j<width-4;j++,p++) {
00226 wider e = p[4];
00227 p[2] = (T)(((a+e)*A + (b+d)*B + c*C)/S);
00228 a = b; b = c; c = d; d = e;
00229 }
00230 p[2] = (T)(((a+c)*A + (b+d)*B + c*C) /S);
00231 p[3] = (T)(((b+b)*A + (c+c)*B + d*C) /S);
00232 p += 4;
00233 }
00234 for (j=0;j<width;j++) {
00235 p = I.data()+j;
00236 wider a = p[0];
00237 wider b = p[width];
00238 p[0] = (T)(((p[2*width]+p[2*width])*A+(b+b)*B + a*C) /S);
00239 p[width] = (T)(((b+p[width*3])*A+(a+p[2*width])*B + b*C) /S);
00240 for (i=0;i<height-4;i++) {
00241 wider c = p[2*width];
00242 p[2*width] = (T)(((a+p[4*width])*A + (b+p[3*width])*B + c*C)/S);
00243 a=b; b=c;
00244 p += width;
00245 }
00246 wider c = p[2*width];
00247 p[2*width] = (T)(((a+c)*A + (b+p[width*3])*B + c*C) /S);
00248 p[3*width] = (T)(((b+b)*A + (c+c)*B + p[width*3]*C) /S);
00249 }
00250 }
00251
00252 template <class T, int A, int B, int C, int D> void convolveSymmetric(Image<T>& I)
00253 {
00254 typedef typename Pixel::traits<T>::wider_type wider;
00255 static const wider S = (A+B+C+D+C+B+A);
00256 int width = I.size().x;
00257 int height = I.size().y;
00258 T* p = I.data();
00259 int i,j;
00260 for (i=0; i<height; i++) {
00261 wider a = p[0];
00262 wider b = p[1];
00263 wider c = p[2];
00264 wider d = p[3];
00265 p[0] = (T)(((d+d)*A + (c+c)*B + (b+b)*C + a*D)/S);
00266 p[1] = (T)(((c+p[4])*A + (b+d)*B + (a+c)*C + b*D)/S);
00267 p[2] = (T)(((b+p[5])*A + (a+p[4])*B + (b+d)*C + c*D)/S);
00268 for (j=0;j<width-6;j++,p++) {
00269 d = p[3];
00270 p[3] = (T)(((a+p[6])*A + (b+p[5])*B + (c+p[4])*C + d*D)/S);
00271 a=b; b=c; c=d;
00272 }
00273 d = p[3];
00274 wider e = p[4];
00275 p[3] = (T)(((a+e)*A + (b+p[5])*B + (c+e)*C + d*D)/S);
00276 p[4] = (T)(((b+d)*A + (c+e)*B + (d+p[5])*C + e*D)/S);
00277 p[5] = (T)(((c+c)*A + (d+d)*B + (e+e)*C + p[5]*D)/S);
00278 p += 6;
00279 }
00280 for (j=0;j<width;j++) {
00281 p = I.data()+j;
00282 wider a = p[0];
00283 wider b = p[width];
00284 wider c = p[2*width];
00285 wider d = p[3*width];
00286 p[0] = (T)(((d+d)*A + (c+c)*B + (b+b)*C + a*D)/S);
00287 p[width] = (T)(((c+p[4*width])*A + (b+d)*B + (a+c)*C + b*D)/S);
00288 p[2*width] = (T)(((b+p[5*width])*A + (a+p[4*width])*B + (b+d)*C + c*D)/S);
00289 for (i=0;i<height-6;i++) {
00290 d = p[3*width];
00291 p[3*width] = (T)(((a+p[width*6])*A + (b+p[width*5])*B + (c+p[width*4])*C+d*D)/S);
00292 a=b; b=c; c=d;
00293 p += width;
00294 }
00295 d = p[3*width];
00296 wider e = p[4*width];
00297 p[3*width] = (T)(((a+e)*A + (b+p[5*width])*B + (c+e)*C + d*D)/S);
00298 p[4*width] = (T)(((b+d)*A + (c+e)*B + (d+p[5*width])*C + e*D)/S);
00299 p[5*width] = (T)(((c+c)*A + (d+d)*B + (e+e)*C + p[5*width]*D)/S);
00300 }
00301 }
00302
00303 template <class T, class K> void convolveSeparableSymmetric(Image<T>& I, const std::vector<K>& kernel, K divisor)
00304 {
00305 typedef typename Pixel::traits<T>::wider_type sum_type;
00306 int w = I.size().x;
00307 int h = I.size().y;
00308 int r = (int)kernel.size()/2;
00309 int i,j;
00310 int m;
00311 double factor = 1.0/divisor;
00312 for (j=0;j<w;j++) {
00313 T* src = I.data()+j;
00314 for (i=0; i<h-2*r; i++,src+=w) {
00315 sum_type sum = src[r*w]*kernel[r], v;
00316 for (m=0; m<r; m++)
00317 sum += (src[m*w] + src[(2*r-m)*w]) * kernel[m];
00318 *(src) = static_cast<T>(sum * factor);
00319 }
00320 }
00321 int offset = r*w + r;
00322 for (i=h-2*r-1;i>=0;i--) {
00323 T* src = I[w];
00324 for (j=0;j<w-2*r;j++, src++) {
00325 sum_type sum = src[r] * kernel[r], v;
00326 for (m=0; m<r; m++)
00327 sum += (src[m] + src[2*r-m])*kernel[m];
00328 *(src+offset) = static_cast<T>(sum*factor);
00329 }
00330 }
00331 }
00332
00333
00334 template <class A, class B> struct GetPixelRowTyped {
00335 static inline const B* get(const A* row, int w, B* rowbuf) {
00336 std::copy(row, row+w, rowbuf);
00337 return rowbuf;
00338 }
00339 };
00340
00341 template <class T> struct GetPixelRowTyped<T,T> {
00342 static inline const T* get(const T* row, int , T* ) {
00343 return row;
00344 }
00345 };
00346
00347 template <class A, class B> const B* getPixelRowTyped(const A* row, int n, B* rowbuf) {
00348 return GetPixelRowTyped<A,B>::get(row,n,rowbuf);
00349 }
00350
00351 template <class T, class S> struct CastCopy {
00352 static inline void cast_copy(const T* from, S* to, int count) {
00353 for (int i=0; i<count; i++)
00354 to[i] = static_cast<S>(from[i]);
00355 }
00356 };
00357
00358 template <class T> struct CastCopy<T,T> {
00359 static inline void cast_copy(const T* from, T* to, int count) {
00360 std::copy(from, from+count, to);
00361 }
00362 };
00363
00364 template <class T, class S> inline void cast_copy(const T* from, S* to, int count) { CastCopy<T,S>::cast_copy(from,to,count); }
00365
00366 template <class T, int N=-1, int C = Pixel::Component<T>::count> struct ConvolveMiddle {
00367 template <class S> static inline T at(const T* input, const S& factor, const S* kernel) { return ConvolveMiddle<T,-1,C>::at(input,factor, kernel, N); }
00368 };
00369
00370 template <class T, int N> struct ConvolveMiddle<T,N,1> {
00371 template <class S> static inline T at(const T* input, const S& factor, const S* kernel) { return ConvolveMiddle<T,N-1>::at(input,factor, kernel) + (input[-N]+input[N])*kernel[N-1]; }
00372 };
00373
00374 template <class T> struct ConvolveMiddle<T,-1,1> {
00375 template <class S> static inline T at(const T* input, const S& factor, const S* kernel, int ksize) {
00376 T hsum = *input * factor;
00377 for (int k=0; k<ksize; k++)
00378 hsum += (input[-k-1] + input[k+1]) * kernel[k];
00379 return hsum;
00380 }
00381 };
00382
00383 template <class T, int C> struct ConvolveMiddle<T,-1, C> {
00384 template <class S> static inline T at(const T* input, const S& factor, const S* kernel, int ksize) {
00385 T hsum = *input * factor;
00386 for (int k=0; k<ksize; k++)
00387 hsum += (input[-k-1] + input[k+1]) * kernel[k];
00388 return hsum;
00389 }
00390 };
00391
00392 template <class T> struct ConvolveMiddle<T,0,1> {
00393 template <class S> static inline T at(const T* input, const S& factor, const S* ) { return *input * factor; }
00394 };
00395
00396 template <class T,class S> inline const T* convolveMiddle(const T* input, const S& factor, const S* kernel, int ksize, int n, T* output) {
00397 #define CALL_CM(I) for (int j=0; j<n; ++j, ++input, ++output) { *output = ConvolveMiddle<T,I>::at(input, factor, kernel); } break
00398
00399 switch (ksize) {
00400 case 0: CALL_CM(0);
00401 case 1: CALL_CM(1);
00402 case 2: CALL_CM(2);
00403 case 3: CALL_CM(3);
00404 case 4: CALL_CM(4);
00405 case 5: CALL_CM(5);
00406 case 6: CALL_CM(6);
00407 case 7: CALL_CM(7);
00408 case 8: CALL_CM(8);
00409 default: for (int j=0; j<n; j++, input++) { *(output++) = ConvolveMiddle<T,-1>::at(input, factor, kernel, ksize); }
00410 }
00411 return input;
00412 #undef CALL_CM
00413 }
00414
00415
00416 template <class T> inline void convolveGaussian(BasicImage<T>& I, double sigma, double sigmas=3.0)
00417 {
00418 convolveGaussian(I,I,sigma,sigmas);
00419 }
00420
00421 template <class T> void convolveGaussian(const BasicImage<T>& I, BasicImage<T>& out, double sigma, double sigmas=3.0)
00422 {
00423 typedef typename Pixel::traits<typename Pixel::Component<T>::type>::float_type sum_comp_type;
00424 typedef typename Pixel::traits<T>::float_type sum_type;
00425 assert(out.size() == I.size());
00426 int ksize = (int)ceil(sigmas*sigma);
00427
00428 std::vector<sum_comp_type> kernel(ksize);
00429 sum_comp_type ksum = sum_comp_type();
00430 for (int i=1; i<=ksize; i++)
00431 ksum += (kernel[i-1] = static_cast<sum_comp_type>(exp(-i*i/(2*sigma*sigma))));
00432 for (int i=0; i<ksize; i++)
00433 kernel[i] /= (2*ksum+1);
00434 double factor = 1.0/(2*ksum+1);
00435 int w = I.size().x;
00436 int h = I.size().y;
00437 int swin = 2*ksize;
00438
00439 AlignedMem<sum_type,16> buffer(w*(swin+1));
00440 AlignedMem<sum_type,16> aligned_rowbuf(w);
00441 AlignedMem<sum_type,16> aligned_outbuf(w);
00442
00443 sum_type* rowbuf = aligned_rowbuf.data();
00444 sum_type* outbuf = aligned_outbuf.data();
00445
00446 std::vector<sum_type*> rows(swin+1);
00447 for (int k=0;k<swin+1;k++)
00448 rows[k] = buffer.data() + k*w;
00449
00450 T* output = out.data();
00451 for (int i=0; i<h; i++) {
00452 sum_type* next_row = rows[swin];
00453 const sum_type* input = getPixelRowTyped(I[i], w, rowbuf);
00454
00455 for (int j=0; j<ksize; j++) {
00456 sum_type hsum = static_cast<sum_type>(input[j] * factor);
00457 for (int k=0; k<ksize; k++)
00458 hsum += (input[std::max(j-k-1,0)] + input[j+k+1]) * kernel[k];
00459 next_row[j] = hsum;
00460 }
00461
00462 input += ksize;
00463 input = convolveMiddle<sum_type, sum_comp_type>(input, static_cast<sum_comp_type>(factor), &kernel.front(), ksize, w-swin, next_row+ksize);
00464
00465 for (int j=w-ksize; j<w; j++, input++) {
00466 sum_type hsum = static_cast<sum_type>(*input * factor);
00467 const int room = w-j;
00468 for (int k=0; k<ksize; k++) {
00469 hsum += (input[-k-1] + input[std::min(k+1,room-1)]) * kernel[k];
00470 }
00471 next_row[j] = hsum;
00472 }
00473
00474 if (i >= swin) {
00475 const sum_type* middle_row = rows[ksize];
00476 assign_multiple(middle_row, factor, outbuf, w);
00477 for (int k=0; k<ksize; k++) {
00478 const sum_comp_type m = kernel[k];
00479 const sum_type* row1 = rows[ksize-k-1];
00480 const sum_type* row2 = rows[ksize+k+1];
00481 add_multiple_of_sum(row1, row2, m, outbuf, w);
00482 }
00483 cast_copy(outbuf, output, w);
00484 output += w;
00485 if (i == h-1) {
00486 for (int r=0; r<ksize; r++) {
00487 const sum_type* middle_row = rows[ksize+r+1];
00488 assign_multiple(middle_row, factor, outbuf, w);
00489 for (int k=0; k<ksize; k++) {
00490 const sum_comp_type m = kernel[k];
00491 const sum_type* row1 = rows[ksize+r-k];
00492 const sum_type* row2 = rows[std::min(ksize+r+k+2, swin)];
00493 add_multiple_of_sum(row1, row2, m, outbuf, w);
00494 }
00495 cast_copy(outbuf, output, w);
00496 output += w;
00497 }
00498 }
00499 } else if (i == swin-1) {
00500 for (int r=0; r<ksize; r++) {
00501 const sum_type* middle_row = rows[r+1];
00502 assign_multiple(middle_row, factor, outbuf, w);
00503 for (int k=0; k<ksize; k++) {
00504 const sum_comp_type m = kernel[k];
00505 const sum_type* row1 = rows[std::max(r-k-1,0)+1];
00506 const sum_type* row2 = rows[r+k+2];
00507 add_multiple_of_sum(row1, row2, m, outbuf, w);
00508 }
00509 cast_copy(outbuf, output, w);
00510 output += w;
00511 }
00512 }
00513
00514 sum_type* tmp = rows[0];
00515 for (int r=0;r<swin; r++)
00516 rows[r] = rows[r+1];
00517 rows[swin] = tmp;
00518 }
00519 }
00520
00521 void compute_van_vliet_b(double sigma, double b[]);
00522 void compute_triggs_M(const double b[], double M[][3]);
00523 void van_vliet_blur(const double b[], const SubImage<float> in, SubImage<float> out);
00524
00525 void convolveGaussian(const BasicImage<float>& I, BasicImage<float>& out, double sigma, double sigmas=3.0);
00526 void convolveGaussian_fir(const BasicImage<float>& I, BasicImage<float>& out, double sigma, double sigmas=3.0);
00527
00528 template <class T, class O, class K> void convolve_gaussian_3(const BasicImage<T>& I, BasicImage<O>& out, K k1, K k2)
00529 {
00530 assert(I.size() == out.size());
00531 const T* a=I.data();
00532 const int w = I.size().x;
00533 O* o = out.data()+w+1;
00534 int total = I.totalsize() - 2*w-2;
00535 const double cross = k1*k2;
00536 k1 *= k1;
00537 k2 *= k2;
00538 while (total--) {
00539 const double sum = k1*(a[0] + a[2] + a[w*2] + a[w*2+2]) + cross*(a[1] + a[w*2+1] + a[w] + a[w+2]) + k2*a[w+1];
00540 *o++ = Pixel::scalar_convert<O,T,double>(sum);
00541 ++a;
00542 }
00543 }
00544
00545 }
00546
00547 #endif