00001 #include "brip_vil1_float_ops.h"
00002
00003
00004
00005 #include <vcl_fstream.h>
00006 #include <vul/vul_timer.h>
00007 #include <vnl/vnl_numeric_traits.h>
00008 #include <vnl/vnl_math.h>
00009 #include <vcl_complex.h>
00010 #include <vbl/vbl_array_1d.h>
00011 #include <vnl/algo/vnl_fft_prime_factors.h>
00012 #include <vnl/algo/vnl_svd.h>
00013 #include <vnl/vnl_inverse.h>
00014 #include <vil1/vil1_smooth.h>
00015 #include <vsol/vsol_box_2d.h>
00016 #include <vsol/vsol_polygon_2d_sptr.h>
00017 #include <vsol/vsol_polygon_2d.h>
00018 #include <bsol/bsol_algs.h>
00019 #include <brip/brip_roi.h>
00020
00021
00022
00023
00024 vil1_memory_image_of<float>
00025 brip_vil1_float_ops::convolve(vil1_memory_image_of<float> const & input,
00026 vbl_array_2d<float> const & kernel)
00027 {
00028 const int w = input.width(), h = input.height();
00029 const int kw = kernel.cols();
00030
00031 int n = (kw-1)/2;
00032 vil1_memory_image_of<float> output;
00033 output.resize(w,h);
00034 for (int y = n; y<(h-n); y++)
00035 for (int x = n; x<(w-n); x++)
00036 {
00037 float accum = 0;
00038 for (int j = -n; j<=n; j++)
00039 for (int i = -n; i<=n; i++)
00040 {
00041 float x1 = input(x+i,y+j);
00042 float x2 = kernel[i+n][j+n];
00043 accum += x1*x2;
00044 }
00045 output(x,y)=accum;
00046 }
00047 brip_vil1_float_ops::fill_x_border(output, n, 0.0f);
00048 brip_vil1_float_ops::fill_y_border(output, n, 0.0f);
00049 return output;
00050 }
00051
00052 static void fill_1d_array(vil1_memory_image_of<float> const & input,
00053 const int y, float* output)
00054 {
00055 const int w = input.width();
00056 for (int x = 0; x<w; x++)
00057 output[x] = input(x,y);
00058 }
00059
00060
00061 void brip_vil1_float_ops::half_resolution_1d(const float* input, int width,
00062 const float k0, const float k1,
00063 const float k2, float* output)
00064 {
00065 float w[5];
00066 int n = 0;
00067 for (; n<5; n++)
00068 w[n]=input[n];
00069 output[0]=k0*w[0]+ 2.0f*(k1*w[1] + k2*w[2]);
00070 for (int x = 1; x<width; x++)
00071 {
00072 output[x]=k0*w[2]+ k1*(w[1]+w[3]) + k2*(w[0]+w[4]);
00073
00074 w[0] = w[2]; w[1] = w[3]; w[2] = w[4];
00075
00076 if (x+2<width)
00077 w[3] = input[n++], w[4] = input[n++];
00078 else
00079 w[3] = w[1], w[4] = w[0];
00080 }
00081 }
00082
00083
00084
00085
00086
00087
00088
00089
00090 vil1_memory_image_of<float>
00091 brip_vil1_float_ops::half_resolution(vil1_memory_image_of<float> const & input,
00092 float filter_coef)
00093 {
00094 vul_timer t;
00095 float k0 = filter_coef, k1 = 0.25f*filter_coef, k2 = 0.5f*(0.5f-filter_coef);
00096 const int w = input.width(), h = input.height();
00097 int half_w =(w+1)/2, half_h = (h+1)/2;
00098 vil1_memory_image_of<float> output;
00099 output.resize(half_w, half_h);
00100
00101 int n = 0;
00102 float* in0 = new float[w]; float* in1 = new float[w];
00103 float* in2 = new float[w]; float* in3 = new float[w];
00104 float* in4 = new float[w];
00105
00106 float* out0 = new float[half_w]; float* out1 = new float[half_w];
00107 float* out2 = new float[half_w]; float* out3 = new float[half_w];
00108 float* out4 = new float[half_w];
00109
00110 fill_1d_array(input, n++, in0); fill_1d_array(input, n++, in1);
00111 fill_1d_array(input, n++, in2); fill_1d_array(input, n++, in3);
00112 fill_1d_array(input, n++, in4);
00113
00114
00115 brip_vil1_float_ops::half_resolution_1d(in0, half_w, k0, k1, k2, out0);
00116 brip_vil1_float_ops::half_resolution_1d(in1, half_w, k0, k1, k2, out1);
00117 brip_vil1_float_ops::half_resolution_1d(in2, half_w, k0, k1, k2, out2);
00118 brip_vil1_float_ops::half_resolution_1d(in3, half_w, k0, k1, k2, out3);
00119 brip_vil1_float_ops::half_resolution_1d(in4, half_w, k0, k1, k2, out4);
00120 int x=0, y;
00121
00122 for (;x<half_w;x++)
00123 output(x,0)= k0*out0[x]+ 2.0f*(k1*out1[x]+k2*out2[x]);
00124
00125 for (y=1; y<half_h; y++)
00126 {
00127 for (x=0; x<half_w; x++)
00128 output(x,y) = k0*out2[x]+ k1*(out1[x]+out3[x]) + k2*(out0[x]+out4[x]);
00129
00130 float* temp0 = out0;
00131 float* temp1 = out1;
00132 out0 = out2; out1 = out3; out2 = out4;
00133 out3 = temp0; out4 = temp1;
00134
00135 if (y<half_h-2)
00136 {
00137
00138 fill_1d_array(input, n++, in3);
00139 fill_1d_array(input, n++, in4);
00140 brip_vil1_float_ops::half_resolution_1d(in3, half_w, k0, k1, k2, out3);
00141 brip_vil1_float_ops::half_resolution_1d(in4, half_w, k0, k1, k2, out4);
00142 }
00143 }
00144 delete [] in0; delete [] in1; delete [] in2;
00145 delete [] in3; delete [] in4;
00146 delete [] out0; delete [] out1; delete [] out2;
00147 delete [] out3; delete [] out4;
00148 vcl_cout << "\nDownsample a "<< w <<" x " << h << " image in "<< t.real() << " msecs.\n";
00149 return output;
00150 }
00151
00152 vil1_memory_image_of<vil1_rgb<unsigned char> > brip_vil1_float_ops::
00153 half_resolution(vil1_memory_image_of<vil1_rgb<unsigned char> > const & input,
00154 float filter_coef)
00155 {
00156 int w = input.width(), h = input.height();
00157
00158 vil1_memory_image_of<float> red(w,h), grn(w,h), blu(w,h);
00159 for (int row = 0; row<h; row++)
00160 for (int col = 0; col<w; col++)
00161 {
00162 vil1_rgb<unsigned char> v = input(col,row);
00163 red(col,row) = v.r;
00164 grn(col,row) = v.g;
00165 blu(col,row) = v.b;
00166 }
00167 vil1_memory_image_of<float> red_half =
00168 brip_vil1_float_ops::half_resolution(red, filter_coef);
00169
00170 vil1_memory_image_of<float> grn_half =
00171 brip_vil1_float_ops::half_resolution(grn, filter_coef);
00172
00173 vil1_memory_image_of<float> blu_half =
00174 brip_vil1_float_ops::half_resolution(blu, filter_coef);
00175
00176 vil1_memory_image_of<unsigned char> red_half_char =
00177 brip_vil1_float_ops::convert_to_byte(red_half);
00178
00179 vil1_memory_image_of<unsigned char> grn_half_char =
00180 brip_vil1_float_ops::convert_to_byte(grn_half);
00181
00182 vil1_memory_image_of<unsigned char> blu_half_char =
00183 brip_vil1_float_ops::convert_to_byte(blu_half);
00184
00185 int w2 = red_half.width(), h2 = red_half.height();
00186 vil1_memory_image_of<vil1_rgb<unsigned char> > out(w2,h2);
00187 for (int row = 0; row<h2; row++)
00188 for (int col = 0; col<w2; col++)
00189 {
00190 unsigned char rduc = red_half_char(col,row);
00191 unsigned char gruc = grn_half_char(col,row);
00192 unsigned char bluc = blu_half_char(col,row);
00193 vil1_rgb<unsigned char> v(rduc, gruc, bluc);
00194 out(col, row) = v;
00195 }
00196 return out;
00197 }
00198
00199 vil1_memory_image_of<float>
00200 brip_vil1_float_ops::gaussian(vil1_memory_image_of<float> const & input, float sigma)
00201 {
00202 vil1_memory_image_of<float> output(vil1_smooth_gaussian(input, sigma));
00203 return output;
00204 }
00205
00206
00207
00208
00209 bool brip_vil1_float_ops::
00210 local_maximum(vbl_array_2d<float> const & neighborhood,
00211 int n, float& value)
00212 {
00213 bool local_max = true;
00214 value = 0;
00215 float center = neighborhood[n][n];
00216 for (int y = -n; y<=n; y++)
00217 for (int x = -n; x<=n; x++)
00218 local_max = local_max&&(neighborhood[y+n][x+n]<=center);
00219 if (!local_max)
00220 return false;
00221 value = center;
00222 return true;
00223 }
00224
00225
00226
00227
00228
00229
00230
00231 void brip_vil1_float_ops::
00232 interpolate_center(vbl_array_2d<float> const & neighborhood,
00233 float& dx, float& dy)
00234 {
00235 dx = 0; dy=0;
00236
00237 float n_m1_m1 = neighborhood[0][0];
00238 float n_m1_0 = neighborhood[0][1];
00239 float n_m1_1 = neighborhood[0][2];
00240 float n_0_m1 = neighborhood[1][0];
00241 float n_0_0 = neighborhood[1][1];
00242 float n_0_1 = neighborhood[1][2];
00243 float n_1_m1 = neighborhood[2][0];
00244 float n_1_0 = neighborhood[2][1];
00245 float n_1_1 = neighborhood[2][2];
00246
00247
00248
00249
00250
00251 float Ix =(-n_m1_m1+n_m1_1-n_0_m1+n_0_1-n_1_m1+n_1_1)/6.0f;
00252
00253
00254
00255 float Iy =(-n_m1_m1-n_m1_0-n_m1_1+n_1_m1+n_1_0+n_1_1)/6.0f;
00256
00257
00258
00259 float Ixx = ((n_m1_m1+n_0_m1+n_1_m1+n_m1_1+n_0_1+n_1_1)
00260 -2.0f*(n_m1_0+n_0_0+n_1_0))/3.0f;
00261
00262
00263
00264 float Ixy = (n_m1_m1-n_m1_1+n_1_m1+n_1_1)/4.0f;
00265
00266
00267
00268 float Iyy = ((n_m1_m1+n_m1_0+n_m1_1+n_1_m1+n_1_0+n_1_1)
00269 -2.0f*(n_0_m1 + n_0_0 + n_1_0))/3.0f;
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280 float det = Ixx*Iyy - Ixy*Ixy;
00281
00282 if (det>0)
00283 {
00284 dx = (Iy*Ixy - Ix*Iyy) / det;
00285 dy = (Ix*Ixy - Iy*Ixx) / det;
00286
00287 if (vcl_fabs(dx) > 1.0 || vcl_fabs(dy) > 1.0)
00288 dx = 0; dy = 0;
00289 }
00290 }
00291
00292
00293
00294
00295
00296 void brip_vil1_float_ops::
00297 non_maximum_suppression(vil1_memory_image_of<float> const & input,
00298 const int n,
00299 const float thresh,
00300 vcl_vector<float>& x_pos,
00301 vcl_vector<float>& y_pos,
00302 vcl_vector<float>& value)
00303 {
00304 vul_timer t;
00305 const int N = 2*n+1;
00306 const int w = input.width(), h = input.height();
00307 x_pos.clear(); x_pos.clear(); value.clear();
00308 vbl_array_2d<float> neighborhood(N,N);
00309 for (int y =n; y<h-n; y++)
00310 for (int x = n; x<w-n; x++)
00311 {
00312
00313
00314 if (input(x,y)<thresh)
00315 continue;
00316
00317 for (int i = -n; i<=n; i++)
00318 for (int j = -n; j<=n; j++)
00319 neighborhood.put(j+n,i+n,input(x+i, y+j));
00320
00321 float dx, dy, max_v;
00322 if (brip_vil1_float_ops::local_maximum(neighborhood, n, max_v))
00323 {
00324
00325 brip_vil1_float_ops::interpolate_center(neighborhood, dx, dy);
00326 x_pos.push_back(x+dx);
00327 y_pos.push_back(y+dy);
00328 value.push_back(max_v);
00329 }
00330 }
00331 vcl_cout << "\nCompute non-maximum suppression on a "<< w <<" x " << h << " image in "<< t.real() << " msecs.\n";
00332 }
00333
00334
00335
00336
00337
00338 vil1_memory_image_of<float>
00339 brip_vil1_float_ops::difference(vil1_memory_image_of<float> const & image_1,
00340 vil1_memory_image_of<float> const & image_2)
00341 {
00342 const int w1 = image_1.width(), h1 = image_1.height();
00343 const int w2 = image_2.width(), h2 = image_2.height();
00344 vil1_memory_image_of<float> temp(w1, h1);
00345 if (w1!=w2||h1!=h2)
00346 {
00347 vcl_cout << "In brip_vil1_float_ops::difference(..) - images are not the same dimensions\n";
00348 return temp;
00349 }
00350 vil1_memory_image_of<float> out;
00351 out.resize(w1, h1);
00352 for (int y = 0; y<h1; y++)
00353 for (int x = 0; x<w1; x++)
00354 out(x,y) = image_2(x,y)-image_1(x,y);
00355 return out;
00356 }
00357
00358 vil1_memory_image_of<float>
00359 brip_vil1_float_ops::abs_clip_to_level(vil1_memory_image_of<float> const & image,
00360 const float thresh, const float level)
00361 {
00362 vil1_memory_image_of<float> out;
00363 const int w = image.width(), h = image.height();
00364 out.resize(w, h);
00365 for (int y = 0; y<h; y++)
00366 for (int x = 0; x<w; x++)
00367 {
00368 if (vcl_fabs(image(x,y))>thresh)
00369 out(x,y) = level;
00370 else
00371 out(x,y) = image(x,y);
00372 }
00373 return out;
00374 }
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385 void brip_vil1_float_ops::gradient_3x3(vil1_memory_image_of<float> const & input,
00386 vil1_memory_image_of<float>& grad_x,
00387 vil1_memory_image_of<float>& grad_y)
00388 {
00389 vul_timer t;
00390 const int w = input.width(), h = input.height();
00391 float scale = 1.0f/6.0f;
00392 for (int y = 1; y<h-1; y++)
00393 for (int x = 1; x<w-1; x++)
00394 {
00395 float gx = input(x+1,y-1)+input(x+1,y)+ input(x+1,y-1)
00396 -input(x-1,y-1) -input(x-1,y) -input(x-1,y-1);
00397 float gy = input(x+1,y+1)+input(x,y+1)+ input(x-1,y+1)
00398 -input(x+1,y-1) -input(x,y-1) -input(x-1,y-1);
00399 grad_x(x,y) = scale*gx;
00400 grad_y(x,y) = scale*gy;
00401 }
00402 brip_vil1_float_ops::fill_x_border(grad_x, 1, 0.0f);
00403 brip_vil1_float_ops::fill_y_border(grad_x, 1, 0.0f);
00404 brip_vil1_float_ops::fill_x_border(grad_y, 1, 0.0f);
00405 brip_vil1_float_ops::fill_y_border(grad_y, 1, 0.0f);
00406 #ifdef DEBUG
00407 vcl_cout << "\nCompute Gradient in " << t.real() << " msecs.\n";
00408 #endif
00409 }
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420 void brip_vil1_float_ops::hessian_3x3(vil1_memory_image_of<float> const & input,
00421 vil1_memory_image_of<float>& Ixx,
00422 vil1_memory_image_of<float>& Ixy,
00423 vil1_memory_image_of<float>& Iyy)
00424 {
00425 vul_timer t;
00426 const int w = input.width(), h = input.height();
00427 for (int y = 1; y<h-1; y++)
00428 for (int x = 1; x<w-1; x++)
00429 {
00430 float xx = input(x-1,y-1)+input(x-1,y)+input(x+1,y)+
00431 input(x+1,y-1)+input(x+1,y)+input(x+1,y+1)-
00432 2.0f*(input(x,y-1)+input(x,y)+input(x,y+1));
00433
00434 float xy = (input(x-1,y-1)+input(x+1,y+1))-
00435 (input(x-1,y+1)+input(x+1,y-1));
00436
00437 float yy = input(x-1,y-1)+input(x,y-1)+input(x+1,y-1)+
00438 input(x-1,y+1)+input(x,y+1)+input(x+1,y+1)-
00439 2.0f*(input(x-1,y)+input(x,y)+input(x+1,y));
00440
00441 Ixx(x,y) = xx/3.0f;
00442 Ixy(x,y) = xy/4.0f;
00443 Iyy(x,y) = yy/3.0f;
00444 }
00445 brip_vil1_float_ops::fill_x_border(Ixx, 1, 0.0f);
00446 brip_vil1_float_ops::fill_y_border(Ixx, 1, 0.0f);
00447 brip_vil1_float_ops::fill_x_border(Ixy, 1, 0.0f);
00448 brip_vil1_float_ops::fill_y_border(Ixy, 1, 0.0f);
00449 brip_vil1_float_ops::fill_x_border(Iyy, 1, 0.0f);
00450 brip_vil1_float_ops::fill_y_border(Iyy, 1, 0.0f);
00451 vcl_cout << "\nCompute a hessian matrix "<< w <<" x " << h << " image in "<< t.real() << " msecs.\n";
00452 }
00453
00454 vil1_memory_image_of<float>
00455 brip_vil1_float_ops::beaudet(vil1_memory_image_of<float> const & Ixx,
00456 vil1_memory_image_of<float> const & Ixy,
00457 vil1_memory_image_of<float> const & Iyy)
00458 {
00459 const int w = Ixx.width(), h = Ixx.height();
00460 vil1_memory_image_of<float> output;
00461 output.resize(w, h);
00462 for (int y = 0; y<h; y++)
00463 for (int x = 0; x<w; x++)
00464 {
00465 float xx = Ixx(x,y), xy = Ixy(x,y), yy = Iyy(x,y);
00466
00467
00468 float det = xx*yy-xy*xy;
00469 float tr = xx+yy;
00470 float arg = tr*tr-4.f*det, lambda0 = 0, lambda1=0;
00471 if (arg>0)
00472 {
00473 lambda0 = tr+vcl_sqrt(arg);
00474 lambda1 = tr-vcl_sqrt(arg);
00475 }
00476 output(x,y) = lambda0*lambda1;
00477 }
00478 return output;
00479 }
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494 void
00495 brip_vil1_float_ops::grad_matrix_NxN(vil1_memory_image_of<float> const & input,
00496 const int n,
00497 vil1_memory_image_of<float>& IxIx,
00498 vil1_memory_image_of<float>& IxIy,
00499 vil1_memory_image_of<float>& IyIy)
00500 {
00501 const int w = input.width(), h = input.height();
00502 const int N = (2*n+1)*(2*n+1);
00503 vil1_memory_image_of<float> grad_x, grad_y, output;
00504 grad_x.resize(w,h);
00505 grad_y.resize(w,h);
00506 output.resize(w,h);
00507 brip_vil1_float_ops::gradient_3x3(input, grad_x, grad_y);
00508 vul_timer t;
00509 for (int y = n; y<h-n;y++)
00510 for (int x = n; x<w-n;x++)
00511 {
00512 float xx=0, xy=0, yy=0;
00513 for (int i = -n; i<=n; i++)
00514 for (int j = -n; j<=n; j++)
00515 {
00516 float gx = grad_x(x+i, y+j), gy = grad_y(x+i, y+j);
00517 xx += gx*gx;
00518 xy += gx*gy;
00519 yy += gy*gy;
00520 }
00521 IxIx(x,y) = xx/N;
00522 IxIy(x,y) = xy/N;
00523 IyIy(x,y) = yy/N;
00524 }
00525 brip_vil1_float_ops::fill_x_border(IxIx, n, 0.0f);
00526 brip_vil1_float_ops::fill_y_border(IxIx, n, 0.0f);
00527 brip_vil1_float_ops::fill_x_border(IxIy, n, 0.0f);
00528 brip_vil1_float_ops::fill_y_border(IxIy, n, 0.0f);
00529 brip_vil1_float_ops::fill_x_border(IyIy, n, 0.0f);
00530 brip_vil1_float_ops::fill_y_border(IyIy, n, 0.0f);
00531 vcl_cout << "\nCompute a gradient matrix "<< w <<" x " << h << " image in "<< t.real() << " msecs.\n";
00532 }
00533
00534 vil1_memory_image_of<float>
00535 brip_vil1_float_ops::harris(vil1_memory_image_of<float> const & IxIx,
00536 vil1_memory_image_of<float> const & IxIy,
00537 vil1_memory_image_of<float> const & IyIy,
00538 const double scale)
00539 {
00540 const int w = IxIx.width(), h = IxIx.height();
00541 float norm = 1e-3f;
00542 vil1_memory_image_of<float> output;
00543 output.resize(w, h);
00544 for (int y = 0; y<h; y++)
00545 for (int x = 0; x<w; x++)
00546 {
00547 float xx = IxIx(x,y), xy = IxIy(x,y), yy = IyIy(x,y);
00548 float det = xx*yy-xy*xy, trace = xx+yy;
00549 output(x,y) = float(det - scale*trace*trace)*norm;
00550 }
00551 return output;
00552 }
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568 vil1_memory_image_of<float>
00569 brip_vil1_float_ops::sqrt_grad_singular_values(vil1_memory_image_of<float> & input,
00570 int n)
00571 {
00572 const int N = (2*n+1)*(2*n+1);
00573 const int w = input.width(), h = input.height();
00574 vil1_memory_image_of<float> grad_x, grad_y, output;
00575 grad_x.resize(w,h);
00576 grad_y.resize(w,h);
00577 output.resize(w,h);
00578 brip_vil1_float_ops::gradient_3x3(input, grad_x, grad_y);
00579 vul_timer t;
00580 for (int y = n; y<h-n;y++)
00581 for (int x = n; x<w-n;x++)
00582 {
00583 float IxIx=0, IxIy=0, IyIy=0;
00584 for (int i = -n; i<=n; i++)
00585 for (int j = -n; j<=n; j++)
00586 {
00587 float gx = grad_x(x+i, y+j), gy = grad_y(x+i, y+j);
00588 IxIx += gx*gx;
00589 IxIy += gx*gy;
00590 IyIy += gy*gy;
00591 }
00592 float det = (IxIx*IyIy-IxIy*IxIy)/N;
00593 output(x,y)=vcl_sqrt(vcl_fabs(det));
00594 }
00595 brip_vil1_float_ops::fill_x_border(output, n, 0.0f);
00596 brip_vil1_float_ops::fill_y_border(output, n, 0.0f);
00597 vcl_cout << "\nCompute sqrt(sigma0*sigma1) in" << t.real() << " msecs.\n";
00598 return output;
00599 }
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609 void
00610 brip_vil1_float_ops::Lucas_KanadeMotion(vil1_memory_image_of<float> & current_frame,
00611 vil1_memory_image_of<float> & previous_frame,
00612 int n, double thresh,
00613 vil1_memory_image_of<float>& vx,
00614 vil1_memory_image_of<float>& vy)
00615 {
00616 const int N = (2*n+1)*(2*n+1);
00617 const int w = current_frame.width(), h = current_frame.height();
00618 vil1_memory_image_of<float> grad_x, grad_y, diff;
00619 grad_x.resize(w,h);
00620 grad_y.resize(w,h);
00621
00622 brip_vil1_float_ops::gradient_3x3(current_frame, grad_x, grad_y);
00623 diff = brip_vil1_float_ops::difference(previous_frame, current_frame);
00624 vul_timer t;
00625
00626 for (int y = n; y<h-n;y++)
00627 for (int x = n; x<w-n;x++)
00628 {
00629 float IxIx=0, IxIy=0, IyIy=0, IxIt=0, IyIt=0;
00630 for (int i = -n; i<=n; i++)
00631 for (int j = -n; j<=n; j++)
00632 {
00633 float gx = grad_x(x+i, y+j), gy = grad_y(x+i, y+j);
00634 float dt = diff(x+i, y+j);
00635 IxIx += gx*gx;
00636 IxIy += gx*gy;
00637 IyIy += gy*gy;
00638 IxIt += gx*dt;
00639 IyIt += gy*dt;
00640 }
00641
00642 IxIx/=N; IxIy/=N; IyIy/=N; IxIt/=N; IyIt/=N;
00643 float det = float(IxIx*IyIy-IxIy*IxIy);
00644
00645 float dif = diff(x,y);
00646 float motion_factor = vcl_fabs(det*dif);
00647 if (motion_factor<thresh)
00648 {
00649 vx(x,y) = 0.0f;
00650 vy(x,y) = 0.0f;
00651 continue;
00652 }
00653
00654 vx(x,y) = (IyIy*IxIt-IxIy*IyIt)/det;
00655 vy(x,y) = (-IxIy*IxIt + IxIx*IyIt)/det;
00656 }
00657 brip_vil1_float_ops::fill_x_border(vx, n, 0.0f);
00658 brip_vil1_float_ops::fill_y_border(vx, n, 0.0f);
00659 brip_vil1_float_ops::fill_x_border(vy, n, 0.0f);
00660 brip_vil1_float_ops::fill_y_border(vy, n, 0.0f);
00661 vcl_cout << "\nCompute Lucas-Kanade in " << t.real() << " msecs.\n";
00662 }
00663
00664 void brip_vil1_float_ops::fill_x_border(vil1_memory_image_of<float> & image,
00665 int w, float value)
00666 {
00667 const int width = image.width(), height = image.height();
00668 if (2*w>width)
00669 {
00670 vcl_cout << "In brip_vil1_float_ops::fill_x_border(..) - 2xborder exceeds image width\n";
00671 return;
00672 }
00673 for (int y = 0; y<height; y++)
00674 for (int x = 0; x<w; x++)
00675 image(x, y) = value;
00676
00677 for (int y = 0; y<height; y++)
00678 for (int x = width-w; x<width; x++)
00679 image(x, y) = value;
00680 }
00681
00682 void brip_vil1_float_ops::fill_y_border(vil1_memory_image_of<float> & image,
00683 int h, float value)
00684 {
00685 const int width = image.width(), height = image.height();
00686 if (2*h>height)
00687 {
00688 vcl_cout << "In brip_vil1_float_ops::fill_y_border(..) - 2xborder exceeds image height\n";
00689 return;
00690 }
00691 for (int y = 0; y<h; y++)
00692 for (int x = 0; x<width; x++)
00693 image(x, y) = value;
00694
00695 for (int y = height-h; y<height; y++)
00696 for (int x = 0; x<width; x++)
00697 image(x, y) = value;
00698 }
00699
00700 vil1_memory_image_of<unsigned char>
00701 brip_vil1_float_ops::convert_to_byte(vil1_memory_image_of<float> const & image)
00702 {
00703
00704 float min_val = vnl_numeric_traits<float>::maxval;
00705 float max_val = -min_val;
00706 const int w = image.width(), h = image.height();
00707 vil1_memory_image_of<unsigned char> output;
00708 output.resize(w,h);
00709 for (int y = 0; y<h; y++)
00710 for (int x = 0; x<w; x++)
00711 {
00712 min_val = vnl_math_min(min_val, image(x,y));
00713 max_val = vnl_math_max(max_val, image(x,y));
00714 }
00715 float range = max_val-min_val;
00716 if (range == 0.f)
00717 range = 1.f;
00718 else
00719 range = 255.f/range;
00720 for (int y = 0; y<h; y++)
00721 for (int x = 0; x<w; x++)
00722 output(x,y) = (unsigned char)((image(x,y)-min_val)*range);
00723 return output;
00724 }
00725
00726
00727
00728 vil1_memory_image_of<unsigned char>
00729 brip_vil1_float_ops::convert_to_byte(vil1_memory_image_of<float> const & image,
00730 const float min_val, const float max_val)
00731 {
00732 const int w = image.width(), h = image.height();
00733 vil1_memory_image_of<unsigned char> output;
00734 output.resize(w,h);
00735 float range = max_val-min_val;
00736 if (range == 0.f)
00737 range = 1.f;
00738 else
00739 range = 255.f/range;
00740 for (int y = 0; y<h; y++)
00741 for (int x = 0; x<w; x++)
00742 {
00743 float v = (image(x,y)-min_val)*range;
00744 if (v>255)
00745 v=255;
00746 if (v<0)
00747 v=0;
00748 output(x,y) = (unsigned char)v;
00749 }
00750 return output;
00751 }
00752
00753 vil1_memory_image_of<unsigned short>
00754 brip_vil1_float_ops::convert_to_short(vil1_memory_image_of<float> const & image,
00755 const float min_val, const float max_val)
00756 {
00757 const int w = image.width(), h = image.height();
00758 float max_short = 65355.f;
00759 vil1_memory_image_of<unsigned short> output;
00760 output.resize(w,h);
00761 float range = max_val-min_val;
00762 if (!range)
00763 range = 1;
00764 else
00765 range = max_short/range;
00766 for (int y = 0; y<h; y++)
00767 for (int x = 0; x<w; x++)
00768 {
00769 float v = (image(x,y)-min_val)*range;
00770 if (v>max_short)
00771 v=max_short;
00772 if (v<0)
00773 v=0;
00774 output(x,y) = (unsigned short)v;
00775 }
00776 return output;
00777 }
00778
00779 vil1_memory_image_of<vil1_rgb<unsigned char> >
00780 brip_vil1_float_ops::convert_to_rgb(vil1_memory_image_of<float> const & image)
00781 {
00782 vil1_memory_image_of<unsigned char> temp = brip_vil1_float_ops::convert_to_byte(image);
00783 const int w = temp.width(), h = temp.height();
00784 vil1_memory_image_of<vil1_rgb<unsigned char> > out(w, h);
00785 for (int r = 0; r<h; r++)
00786 for (int c = 0; c<w; c++)
00787 out(c,r).r = out(c,r).g = out(c,r).b = temp(c,r);
00788 return out;
00789 }
00790
00791 vil1_memory_image_of<vil1_rgb<unsigned char> >
00792 brip_vil1_float_ops::convert_to_rgb(vil1_memory_image_of<float> const & image,
00793 const float min_val, const float max_val)
00794 {
00795 vil1_memory_image_of<unsigned char> temp = brip_vil1_float_ops::convert_to_byte(image,min_val,max_val);
00796 const int w = temp.width(), h = temp.height();
00797 vil1_memory_image_of<vil1_rgb<unsigned char> > out(w, h);
00798 for (int r = 0; r<h; r++)
00799 for (int c = 0; c<w; c++)
00800 out(c,r).r = out(c,r).g = out(c,r).b = temp(c,r);
00801 return out;
00802 }
00803
00804
00805 vil1_memory_image_of<float>
00806 brip_vil1_float_ops::convert_to_float(vil1_memory_image_of<unsigned char> const & image)
00807 {
00808 vil1_memory_image_of<float> output;
00809 const int w = image.width(), h = image.height();
00810 output.resize(w,h);
00811 for (int y = 0; y<h; y++)
00812 for (int x = 0; x<w; x++)
00813 output(x,y) = (float)image(x,y);
00814 return output;
00815 }
00816
00817 vil1_memory_image_of<float>
00818 brip_vil1_float_ops::convert_to_float(vil1_memory_image_of<vil1_rgb<unsigned char> > const & image)
00819 {
00820 vil1_memory_image_of<float> output;
00821 const int w = image.width(), h = image.height();
00822 output.resize(w,h);
00823 for (int y = 0; y<h; y++)
00824 for (int x = 0; x<w; x++)
00825 output(x,y) = (float)image(x,y).grey();
00826 return output;
00827 }
00828
00829 vil1_memory_image_of<float>
00830 brip_vil1_float_ops::convert_to_float(vnl_matrix<float> const & matrix)
00831 {
00832 unsigned int nr = matrix.rows(), nc = matrix.cols();
00833 vil1_memory_image_of<float> out(nc, nr);
00834 for (unsigned int r = 0; r<nr; ++r)
00835 for (unsigned int c = 0; c<nc; ++c)
00836 out(c,r)=matrix[r][c];
00837 return out;
00838 }
00839
00840 static void rgb_to_ihs(vil1_rgb<unsigned char> const & rgb,
00841 float& i, float& h, float& s)
00842 {
00843
00844 float r = rgb.R(), g = rgb.G(), b = rgb.B();
00845 i = rgb.grey();
00846
00847 float maxval = vnl_math_max(r, vnl_math_max(g, b));
00848 float minval = vnl_math_min(r, vnl_math_min(g, b));
00849
00850
00851 float la = (maxval + minval) / 2.f;
00852
00853 if (maxval == minval||i<20||i>235)
00854 {
00855 s = -1.f;
00856 h = 0.f;
00857 }
00858 else
00859 {
00860
00861 if (la <= 127)
00862 {
00863 s = (255.f*(maxval - minval))/(maxval + minval);
00864 }
00865 else
00866 {
00867 s = (255.f*(maxval - minval)) / (512.f - maxval - minval);
00868 if (s<0)
00869 s = 0.f;
00870 if (s>255)
00871 s = 255.f;
00872 }
00873
00874
00875 float delta = maxval - minval;
00876 if (r == maxval)
00877 {
00878
00879 h = (60*(g - b))/ delta;
00880 }
00881 else if (g == maxval)
00882 {
00883
00884 h = 120.f + (60*(b - r))/delta;
00885 }
00886 else
00887 {
00888
00889 h = 240.f + (60*(r - g))/delta;
00890 }
00891
00892 if (h < 0)
00893 h += 360.f;
00894 if (h > 360)
00895 h -= 360.f;
00896 }
00897 }
00898
00899
00900 void brip_vil1_float_ops::
00901 convert_to_IHS(vil1_memory_image_of<vil1_rgb<unsigned char> >const& image,
00902 vil1_memory_image_of<float>& I,
00903 vil1_memory_image_of<float>& H,
00904 vil1_memory_image_of<float>& S)
00905 {
00906 const int w = image.width(), h = image.height();
00907 I.resize(w,h);
00908 H.resize(w,h);
00909 S.resize(w,h);
00910 for (int r = 0; r < h; r++)
00911 for (int c = 0; c < w; c++)
00912 {
00913 float in, hue, sat;
00914 rgb_to_ihs(image(c,r), in, hue, sat);
00915 I(c,r) = in;
00916 H(c,r) = hue;
00917 S(c,r) = sat;
00918 }
00919 }
00920
00921 #if 0 // this method commented out
00922 void brip_vil1_float_ops::
00923 display_IHS_as_RGB(vil1_memory_image_of<float> const& I,
00924 vil1_memory_image_of<float> const& H,
00925 vil1_memory_image_of<float> const& S,
00926 vil1_memory_image_of<vil1_rgb<unsigned char> >& image)
00927 {
00928 const int w = I.width(), h = I.height();
00929 image.resize(w,h);
00930 float s = 255.0f/360.0f;
00931 for (int r = 0; r < h; r++)
00932 for (int c = 0; c < w; c++)
00933 {
00934 float in, hue, sat;
00935 in = I(c,r);
00936 hue = H(c,r);
00937 sat = S(c,r);
00938 if (in<0)
00939 in = 0;
00940 if (sat<0)
00941 sat = 0;
00942 if (hue<0)
00943 hue = 0;
00944 if (in>255)
00945 in = 255;
00946 hue *=s;
00947 if (hue>255)
00948 hue = 255;
00949 if (sat>255)
00950 sat = 255;
00951 unsigned char vi = (unsigned char)in, vh = (unsigned char)hue, vs = (unsigned char)sat;
00952 image(c,r) = vil1_rgb<unsigned char>(vi, vh, vs);
00953 }
00954 }
00955 #endif // 0
00956
00957
00958 void brip_vil1_float_ops::
00959 display_IHS_as_RGB(vil1_memory_image_of<float> const& I,
00960 vil1_memory_image_of<float> const& H,
00961 vil1_memory_image_of<float> const& S,
00962 vil1_memory_image_of<vil1_rgb<unsigned char> >& image)
00963 {
00964 const int w = I.width(), h = I.height();
00965 image.resize(w,h);
00966
00967 const float deg_to_rad = float(vnl_math::pi_over_180);
00968 for (int r = 0; r < h; r++)
00969 for (int c = 0; c < w; c++)
00970 {
00971 float hue = H(c,r);
00972 float sat = 2.f*S(c,r);
00973 if (sat<0)
00974 sat = 0.f;
00975 if (sat>255)
00976 sat = 255.f;
00977 float ang = deg_to_rad*hue;
00978 float cs = vcl_cos(ang), si = vcl_fabs(vcl_sin(ang));
00979 float red,green,blue;
00980 green = si*sat;
00981 if (cs>=0)
00982 {
00983 red = cs*sat;
00984 blue = 0;
00985 }
00986 else
00987 {
00988 red = 0;
00989 blue = sat*(-cs);
00990 }
00991 unsigned char rc = (unsigned char)red,
00992 gc = (unsigned char)green, bc = (unsigned char)blue;
00993 image(c,r)= vil1_rgb<unsigned char>(rc, gc, bc);
00994 }
00995 }
00996
00997 vil1_memory_image_of<float>
00998 brip_vil1_float_ops::convert_to_float(vil1_image const & image)
00999 {
01000 vil1_memory_image_of<float> fimg;
01001 if (image.components()==1)
01002 {
01003 if (image.component_format()==VIL1_COMPONENT_FORMAT_IEEE_FLOAT)
01004
01005 {
01006 fimg = vil1_memory_image_of<float>(image);
01007 return fimg;
01008 }
01009 vil1_memory_image_of<unsigned char> temp(image);
01010 fimg = brip_vil1_float_ops::convert_to_float(temp);
01011 }
01012 else if (image.components()==3)
01013 {
01014 vil1_memory_image_of<vil1_rgb<unsigned char> > temp(image);
01015 fimg = brip_vil1_float_ops::convert_to_float(temp);
01016 }
01017 else
01018 {
01019 vcl_cout << "In brip_vil1_float_ops::convert_to_float - input not color or grey\n";
01020 return vil1_memory_image_of<float>();
01021 }
01022 return fimg;
01023 }
01024
01025
01026
01027 vil1_memory_image_of<unsigned char>
01028 brip_vil1_float_ops::convert_to_grey(vil1_image const& image)
01029 {
01030 if (!image)
01031 return vil1_memory_image_of<unsigned char>();
01032
01033
01034 if (image.components()==1 &&
01035 image.component_format()==VIL1_COMPONENT_FORMAT_IEEE_FLOAT)
01036 return brip_vil1_float_ops::convert_to_byte(vil1_memory_image_of<float>(image));
01037
01038
01039
01040 if (image.components()!=3)
01041 return vil1_memory_image_of<unsigned char>(image);
01042
01043
01044
01045 vil1_memory_image_of<vil1_rgb<unsigned char> > color_image(image);
01046 const int width = color_image.width(), height = color_image.height();
01047
01048 vil1_memory_image_of<unsigned char> grey_image;
01049 grey_image.resize(width, height);
01050 for (int y = 0; y<height; y++)
01051 for (int x = 0; x<width; x++)
01052 grey_image(x,y) = color_image(x,y).grey();
01053 return grey_image;
01054 }
01055
01056
01057
01058
01059
01060
01061
01062
01063
01064
01065
01066 vbl_array_2d<float> brip_vil1_float_ops::load_kernel(vcl_string const & file)
01067 {
01068 vcl_ifstream instr(file.c_str(), vcl_ios::in);
01069 if (!instr)
01070 {
01071 vcl_cout << "In brip_vil1_float_ops::load_kernel - failed to load kernel\n";
01072 return vbl_array_2d<float>(0,0);
01073 }
01074 int n;
01075 float scale;
01076 float v =0;
01077 instr >> n;
01078 instr >> scale;
01079 int N = 2*n+1;
01080 vbl_array_2d<float> output(N, N);
01081 for (int y = 0; y<N; y++)
01082 for (int x = 0; x<N; x++)
01083 {
01084 instr >> v;
01085 output.put(x, y, v/scale);
01086 }
01087 vcl_cout << "The Kernel\n";
01088 for (int y = 0; y<N; y++)
01089 {
01090 for (int x = 0; x<N; x++)
01091 vcl_cout << ' ' << output[x][y];
01092 vcl_cout << '\n';
01093 }
01094 return output;
01095 }
01096
01097 static void insert_image(vil1_memory_image_of<float> const& image, int col,
01098 vnl_matrix<float> & I)
01099 {
01100 const int width = image.width(), height = image.height();
01101 for (int y = 0, row = 0; y<height; ++y)
01102 for (int x = 0; x<width; x++, ++row)
01103 I.put(row, col, image(x,y));
01104 }
01105
01106 void brip_vil1_float_ops::
01107 basis_images(vcl_vector<vil1_memory_image_of<float> > const & input_images,
01108 vcl_vector<vil1_memory_image_of<float> > & basis)
01109 {
01110 basis.clear();
01111 int n_images = input_images.size();
01112 if (!n_images)
01113 {
01114 vcl_cout << "In brip_vil1_float_ops::basis_images(.) - no input images\n";
01115 return;
01116 }
01117 const int width = input_images[0].width(), height = input_images[0].height();
01118 const int npix = width*height;
01119
01120
01121 vnl_matrix<float> I(npix, n_images, 0.f);
01122 for (int i = 0; i<n_images; i++)
01123 insert_image(input_images[i], i, I);
01124
01125
01126 vcl_cout << "Computing Singular values of a " << npix << " by "
01127 << n_images << " matrix\n";
01128 vul_timer t;
01129 vnl_svd<float> svd(I);
01130 vcl_cout << "SVD Took " << t.real() << " msecs\n"
01131 << "Eigenvalues:\n";
01132 for (int i = 0; i<n_images; i++)
01133 vcl_cout << svd.W(i) << '\n';
01134
01135
01136 int rank = svd.rank();
01137 if (!rank)
01138 {
01139 vcl_cout << "In brip_vil1_float_ops::basis_images(.) - I has zero rank\n";
01140 return;
01141 }
01142 vnl_matrix<float> U = svd.U();
01143
01144 int rows = U.rows();
01145 for (int k = 0; k<rank; k++)
01146 {
01147 vil1_memory_image_of<float> out(width, height);
01148 int x =0, y = 0;
01149 for (int r = 0; r<rows; r++)
01150 {
01151 out(x, y) = U(r,k);
01152 x++;
01153 if (x>=width)
01154 {
01155 y++;
01156 x=0;
01157 }
01158 if (y>=width)
01159 {
01160 vcl_cout << "In brip_vil1_float_ops::basis_images(.) - shouldn't happen\n";
01161 return;
01162 }
01163 }
01164 basis.push_back(out);
01165 }
01166 }
01167
01168
01169
01170
01171
01172
01173
01174
01175
01176
01177
01178
01179
01180
01181
01182
01183
01184
01185
01186
01187
01188
01189
01190
01191
01192
01193 bool brip_vil1_float_ops::fft_1d(int dir, int m, double* x, double* y)
01194 {
01195 long nn,i,i1,j,k,i2,l,l1,l2;
01196 double c1,c2,tx,ty,t1,t2,u1,u2,z;
01197
01198
01199 nn = 1;
01200 for (i=0;i<m;i++)
01201 nn *= 2;
01202
01203
01204 i2 = nn >> 1;
01205 j = 0;
01206 for (i=0;i<nn-1;i++) {
01207 if (i < j) {
01208 tx = x[i];
01209 ty = y[i];
01210 x[i] = x[j];
01211 y[i] = y[j];
01212 x[j] = tx;
01213 y[j] = ty;
01214 }
01215 k = i2;
01216 while (k <= j) {
01217 j -= k;
01218 k >>= 1;
01219 }
01220 j += k;
01221 }
01222
01223
01224 c1 = -1.0;
01225 c2 = 0.0;
01226 l2 = 1;
01227 for (l=0;l<m;l++) {
01228 l1 = l2;
01229 l2 <<= 1;
01230 u1 = 1.0;
01231 u2 = 0.0;
01232 for (j=0;j<l1;j++) {
01233 for (i=j;i<nn;i+=l2) {
01234 i1 = i + l1;
01235 t1 = u1 * x[i1] - u2 * y[i1];
01236 t2 = u1 * y[i1] + u2 * x[i1];
01237 x[i1] = x[i] - t1;
01238 y[i1] = y[i] - t2;
01239 x[i] += t1;
01240 y[i] += t2;
01241 }
01242 z = u1 * c1 - u2 * c2;
01243 u2 = u1 * c2 + u2 * c1;
01244 u1 = z;
01245 }
01246 c2 = vcl_sqrt((1.0 - c1) / 2.0);
01247 if (dir == 1)
01248 c2 = -c2;
01249 c1 = vcl_sqrt((1.0 + c1) / 2.0);
01250 }
01251
01252
01253 if (dir == 1) {
01254 for (i=0;i<nn;i++) {
01255 x[i] /= (double)nn;
01256 y[i] /= (double)nn;
01257 }
01258 }
01259
01260 return true;
01261 }
01262
01263
01264
01265
01266
01267
01268
01269
01270 bool brip_vil1_float_ops::fft_2d(vnl_matrix<vcl_complex<double> >& c,int nx,int ny,int dir)
01271 {
01272 int i,j;
01273 int mx, my;
01274 double *real,*imag;
01275 vnl_fft_prime_factors<double> pfx (nx);
01276 vnl_fft_prime_factors<double> pfy (ny);
01277 mx = (int)pfx.pqr()[0];
01278 my = (int)pfy.pqr()[0];
01279
01280 real = new double[nx];
01281 imag = new double[nx];
01282 if (real == 0 || imag == 0)
01283 return false;
01284 for (j=0;j<ny;j++) {
01285 for (i=0;i<nx;i++) {
01286 real[i] = c[j][i].real();
01287 imag[i] = c[j][i].imag();
01288 }
01289 brip_vil1_float_ops::fft_1d(dir,mx,real,imag);
01290 for (i=0;i<nx;i++) {
01291 vcl_complex<double> v(real[i], imag[i]);
01292 c[j][i] = v;
01293 }
01294 }
01295 delete [] real;
01296 delete [] imag;
01297
01298 real = new double[ny];
01299 imag = new double[ny];
01300 if (real == 0 || imag == 0)
01301 return false;
01302 for (i=0;i<nx;i++) {
01303 for (j=0;j<ny;j++) {
01304 real[j] = c[j][i].real();
01305 imag[j] = c[j][i].imag();
01306 }
01307 fft_1d(dir,my,real,imag);
01308 for (j=0;j<ny;j++) {
01309 vcl_complex<double> v(real[j], imag[j]);
01310 c[j][i] = v;
01311 }
01312 }
01313 delete [] real;
01314 delete [] imag;
01315 return true;
01316 }
01317
01318
01319
01320 void brip_vil1_float_ops::
01321 ftt_fourier_2d_reorder(vnl_matrix<vcl_complex<double> > const& F1,
01322 vnl_matrix<vcl_complex<double> > & F2)
01323 {
01324 int rows = F1.rows(), cols = F1.cols();
01325 int half_rows = rows/2, half_cols = cols/2;
01326 int ri, ci;
01327 for (int r = 0; r<rows; r++)
01328 {
01329 if (r<half_rows)
01330 ri = half_rows+r;
01331 else
01332 ri = r-half_rows;
01333 for (int c = 0; c<cols; c++)
01334 {
01335 if (c<half_cols)
01336 ci = half_cols+c;
01337 else
01338 ci = c-half_cols;
01339 F2[ri][ci]=F1[r][c];
01340 }
01341 }
01342 }
01343
01344
01345
01346 bool brip_vil1_float_ops::
01347 fourier_transform(vil1_memory_image_of<float> const & input,
01348 vil1_memory_image_of<float>& mag,
01349 vil1_memory_image_of<float>& phase)
01350 {
01351 const int w = input.width(), h = input.height();
01352 vnl_fft_prime_factors<float> pfx (w);
01353 vnl_fft_prime_factors<float> pfy (h);
01354 if (!pfx.pqr()[0]||!pfy.pqr()[0])
01355 return false;
01356
01357 vnl_matrix<vcl_complex<double> > fft_matrix(h, w), fourier_matrix(h,w);
01358 for (int y = 0; y<h; y++)
01359 for (int x =0; x<w; x++)
01360 {
01361 vcl_complex<double> cv(input(x,y), 0.0);
01362 fft_matrix.put(y, x, cv);
01363 }
01364 #ifdef DEBUG
01365 for (int r = 0; r<h; r++)
01366 for (int c =0; c<w; c++)
01367 {
01368 vcl_complex<double> res = fft_matrix[r][c];
01369 vcl_cout << res << '\n';
01370 }
01371 #endif
01372
01373 brip_vil1_float_ops::fft_2d(fft_matrix, w, h, 1);
01374 brip_vil1_float_ops::ftt_fourier_2d_reorder(fft_matrix, fourier_matrix);
01375 mag.resize(w,h);
01376 phase.resize(w,h);
01377
01378
01379 for (int r = 0; r<h; r++)
01380 for (int c = 0; c<w; c++)
01381 {
01382 float re = (float)fourier_matrix[r][c].real(), im = (float)fourier_matrix[r][c].imag();
01383 mag(c,r) = vcl_sqrt(re*re + im*im);
01384 phase(c,r) = vcl_atan2(im, re);
01385 }
01386
01387 return true;
01388 }
01389
01390 bool brip_vil1_float_ops::
01391 inverse_fourier_transform(vil1_memory_image_of<float> const& mag,
01392 vil1_memory_image_of<float> const& phase,
01393 vil1_memory_image_of<float>& output)
01394 {
01395 const int w = mag.width(), h = mag.height();
01396 vnl_matrix<vcl_complex<double> > fft_matrix(h, w), fourier_matrix(h, w);
01397 for (int y = 0; y<h; y++)
01398 for (int x =0; x<w; x++)
01399 {
01400 float m = mag(x,y);
01401 float p = phase(x,y);
01402 vcl_complex<double> cv(m*vcl_cos(p), m*vcl_sin(p));
01403 fourier_matrix.put(y, x, cv);
01404 }
01405
01406 brip_vil1_float_ops::ftt_fourier_2d_reorder(fourier_matrix, fft_matrix);
01407 brip_vil1_float_ops::fft_2d(fft_matrix, w, h, -1);
01408
01409 output.resize(w,h);
01410
01411 for (int y = 0; y<h; y++)
01412 for (int x = 0; x<w; x++)
01413 output(x,y) = (float)fft_matrix[y][x].real();
01414 return true;
01415 }
01416
01417 void brip_vil1_float_ops::resize(vil1_memory_image_of<float> const & input,
01418 const int width, const int height,
01419 vil1_memory_image_of<float>& output)
01420 {
01421 const int w = input.width(), h = input.height();
01422 output.resize(width, height);
01423 for (int y = 0; y<height; y++)
01424 for (int x = 0; x<width; x++)
01425 if (x<w && y<h)
01426 output(x,y) = input(x,y);
01427 else
01428 output(x,y) = 0;
01429 }
01430
01431
01432 bool brip_vil1_float_ops::
01433 resize_to_power_of_two(vil1_memory_image_of<float> const & input,
01434 vil1_memory_image_of<float>& output)
01435 {
01436 const int max_exp = 13;
01437 const int w = input.width(), h = input.height();
01438 int prodw = 1, prodh = 1;
01439
01440 int nw, nh;
01441 for (nw = 1; nw<=max_exp; nw++)
01442 if (prodw>w)
01443 break;
01444 else
01445 prodw *= 2;
01446 if (nw==max_exp)
01447 return false;
01448
01449 for (nh = 1; nh<=max_exp; nh++)
01450 if (prodh>h)
01451 break;
01452 else
01453 prodh *= 2;
01454 if (nh==max_exp)
01455 return false;
01456 brip_vil1_float_ops::resize(input, prodw, prodh, output);
01457
01458 return true;
01459 }
01460
01461
01462
01463
01464
01465
01466
01467
01468 float brip_vil1_float_ops::gaussian_blocking_filter(const float dir_fx,
01469 const float dir_fy,
01470 const float f0,
01471 const float radius,
01472 const float fx,
01473 const float fy)
01474 {
01475
01476 float mag = vcl_sqrt(dir_fx*dir_fx + dir_fy*dir_fy);
01477 if (!mag)
01478 return 0.f;
01479 float r2 = 2.f*radius*radius;
01480 float dx = dir_fx/mag, dy = dir_fy/mag;
01481
01482 float fx0p = dx*f0, fy0p = dy*f0;
01483 float fx0m = -dx*f0, fy0m = -dy*f0;
01484
01485 float d2p = (fx-fx0p)*(fx-fx0p) + (fy-fy0p)*(fy-fy0p);
01486 float d2m = (fx-fx0m)*(fx-fx0m) + (fy-fy0m)*(fy-fy0m);
01487
01488 float d = d2p;
01489 if (d2m<d2p)
01490 d = d2m;
01491
01492 float gb = 1.f-(float)vcl_exp(-d/r2);
01493 return gb;
01494 }
01495
01496 bool brip_vil1_float_ops::
01497 spatial_frequency_filter(vil1_memory_image_of<float> const & input,
01498 const float dir_fx, const float dir_fy,
01499 const float f0, const float radius,
01500 const bool output_fourier_mag,
01501 vil1_memory_image_of<float> & output)
01502 {
01503
01504 vil1_memory_image_of<float> pow_two, mag, bmag, phase, pow_two_filt;
01505 brip_vil1_float_ops::resize_to_power_of_two(input, pow_two);
01506 const int Nfx = pow_two.width(), Nfy = pow_two.height();
01507
01508 if (!brip_vil1_float_ops::fourier_transform(pow_two, mag, phase))
01509 return false;
01510 bmag.resize(Nfx, Nfy);
01511
01512
01513 float Ofx = Nfx*0.5f, Ofy = Nfy*0.5f;
01514 for (int fy =0; fy<Nfy; fy++)
01515 for (int fx =0; fx<Nfx; fx++)
01516 {
01517 float gb = gaussian_blocking_filter(dir_fx, dir_fy, f0,
01518 radius,
01519 fx-Ofx, fy-Ofy);
01520 bmag(fx,fy) = mag(fx,fy)*gb;
01521 }
01522 if (output_fourier_mag)
01523 {
01524 output = bmag;
01525 return true;
01526 }
01527
01528 pow_two_filt.resize(Nfx, Nfy);
01529 brip_vil1_float_ops::inverse_fourier_transform(bmag, phase, pow_two_filt);
01530
01531
01532 brip_vil1_float_ops::resize(pow_two_filt, input.width(), input.height(), output);
01533 return true;
01534 }
01535
01536
01537
01538
01539
01540
01541
01542 float brip_vil1_float_ops::
01543 bilinear_interpolation(vil1_memory_image_of<float> const & input,
01544 const double x, const double y)
01545 {
01546
01547 const int w = input.width(), h = input.height();
01548
01549 int xr = (int)x, yr = (int)y;
01550 double fx = x-xr, fy = y-yr;
01551 if (xr<0||xr>w-2)
01552 return 0.f;
01553 if (yr<0||yr>h-2)
01554 return 0.f;
01555 double int00 = input(xr, yr), int10 = input(xr+1,yr);
01556 double int01 = input(xr, yr+1), int11 = input(xr+1,yr+1);
01557 double int0 = int00 + fy * (int01 - int00);
01558 double int1 = int10 + fy * (int11 - int10);
01559 float val = (float) (int0 + fx * (int1 - int0));
01560 return val;
01561 }
01562
01563
01564
01565
01566 bool brip_vil1_float_ops::homography(vil1_memory_image_of<float> const & input,
01567 vgl_h_matrix_2d<double>const& H,
01568 vil1_memory_image_of<float>& output,
01569 bool output_size_fixed,
01570 float output_fill_value)
01571 {
01572 if (!input)
01573 return false;
01574
01575
01576
01577
01578
01579
01580
01581 vsol_box_2d_sptr input_roi, output_roi;
01582 vsol_polygon_2d_sptr input_poly, output_poly;
01583 vgl_h_matrix_2d<double> Hinv;
01584
01585 const int win = input.width(), hin = input.height();
01586 input_roi = new vsol_box_2d();
01587 input_roi->add_point(0, 0);
01588 input_roi->add_point(win, hin);
01589 input_poly = bsol_algs::poly_from_box(input_roi);
01590
01591
01592
01593 if (!output_size_fixed)
01594 {
01595 if (!bsol_algs::homography(input_poly, H, output_poly))
01596 return false;
01597 vsol_box_2d_sptr temp = output_poly->get_bounding_box();
01598 output.resize((int)temp->width(), (int)temp->height());
01599 output.fill(output_fill_value);
01600
01601 output_roi = new vsol_box_2d();
01602 output_roi->add_point(0, 0);
01603 output_roi->add_point(temp->width(), temp->height());
01604 vnl_matrix_fixed<double,3, 3> Mt = H.get_matrix();
01605 Mt[0][2] -= temp->get_min_x(); Mt[1][2] -= temp->get_min_y();
01606 vnl_matrix_fixed<double,3, 3> Mtinv = vnl_inverse(Mt);
01607 Hinv = vgl_h_matrix_2d<double> (Mtinv);
01608 }
01609 else
01610 {
01611
01612 if (!output)
01613 return false;
01614
01615 int wout = output.width(), hout = output.height();
01616 output.fill(output_fill_value);
01617 output_roi = new vsol_box_2d();
01618 output_roi->add_point(0, 0);
01619 output_roi->add_point(wout, hout);
01620 output_poly = bsol_algs::poly_from_box(output_roi);
01621
01622
01623 vsol_polygon_2d_sptr tpoly;
01624 Hinv = H.get_inverse();
01625 if (!bsol_algs::homography(output_poly, Hinv, tpoly))
01626 return false;
01627
01628
01629 vsol_box_2d_sptr tbox = tpoly->get_bounding_box();
01630
01631
01632 vsol_box_2d_sptr temp;
01633 if (!bsol_algs::intersection(tbox, input_roi, temp))
01634 return false;
01635 input_roi = temp;
01636 }
01637
01638
01639
01640
01641
01642
01643
01644
01645 int ailow = int(input_roi->get_min_x()+0.9999f);
01646 int aihigh = int(input_roi->get_max_x());
01647 int ajlow = int(input_roi->get_min_y()+0.9999f);
01648 int ajhigh = int(input_roi->get_max_y());
01649
01650
01651 int bilow = int(output_roi->get_min_x()+0.9999f);
01652 int bihigh = int(output_roi->get_max_x());
01653 int bjlow = int(output_roi->get_min_y()+0.9999f);
01654 int bjhigh = int(output_roi->get_max_y());
01655
01656
01657 const vnl_matrix_fixed<double,3,3>& Minv = Hinv.get_matrix();
01658
01659
01660 for (int i = bilow; i<bihigh; i++)
01661 for (int j = bjlow; j<bjhigh; j++)
01662 {
01663
01664 float val;
01665 double u = Minv[0][0] * i + Minv[0][1] * j + Minv[0][2];
01666 double v = Minv[1][0] * i + Minv[1][1] * j + Minv[1][2];
01667 double w = Minv[2][0] * i + Minv[2][1] * j + Minv[2][2];
01668 u /= w;
01669 v /= w;
01670
01671
01672 {
01673 int iu = (int) u;
01674 int iv = (int) v;
01675 double fu = u - iu;
01676 double fv = v - iv;
01677
01678 if ((iu < ailow || iu >= aihigh-1) ||
01679 (iv < ajlow || iv >= ajhigh-1))
01680 continue;
01681 else
01682 {
01683
01684
01685
01686
01687 double v00 = input(iu, iv);
01688 double v01 = input(iu, iv+1);
01689 double v10 = input(iu+1,iv);
01690 double v11 = input(iu+1, iv+1);
01691
01692 double v0 = v00 + fv * (v01 - v00);
01693 double v1 = v10 + fv * (v11 - v10);
01694 val = (float) (v0 + fu * (v1 - v0));
01695 }
01696
01697 output(i,j) = val;
01698 }
01699 }
01700 return true;
01701 }
01702
01703
01704
01705 vil1_memory_image_of<float>
01706 brip_vil1_float_ops::rotate(vil1_memory_image_of<float> const & input,
01707 const double theta_deg)
01708 {
01709 vil1_memory_image_of<float> out;
01710 if (!input)
01711 return out;
01712 double ang = theta_deg;
01713
01714 while (ang>360)
01715 ang-=360;
01716 while (ang<0)
01717 ang+=360;
01718
01719 double deg_to_rad = vnl_math::pi_over_180;
01720 double rang = deg_to_rad*ang;
01721 double c = vcl_cos(rang), s = vcl_sin(rang);
01722 vnl_matrix_fixed<double,3, 3> M;
01723
01724 M[0][0]= c; M[0][1]= -s; M[0][2]= 0;
01725 M[1][0]= s; M[1][1]= c; M[1][2]= 0;
01726 M[2][0]= 0; M[2][1]= 0; M[2][2]= 1;
01727 vgl_h_matrix_2d<double> H(M);
01728 vil1_memory_image_of<float> temp;
01729
01730
01731 if (!brip_vil1_float_ops::homography(input, H, temp))
01732 return out;
01733 return temp;
01734 }
01735
01736 bool brip_vil1_float_ops::chip(vil1_memory_image_of<float> const & input,
01737 vsol_box_2d_sptr const& roi,
01738 vil1_memory_image_of<float>& chp)
01739 {
01740 if (!input||!roi)
01741 return false;
01742
01743 const int w = input.width(), h = input.height();
01744 int x_min = (int)roi->get_min_x(), y_min = (int)roi->get_min_y();
01745 int x_max = (int)roi->get_max_x(), y_max = (int)roi->get_max_y();
01746 if (x_min<0)
01747 x_min = 0;
01748 if (y_min<0)
01749 y_min = 0;
01750 if (x_max>w-1)
01751 x_max=w-1;
01752 if (y_max>h-1)
01753 y_max=w-1;
01754 int rw = x_max-x_min, rh = y_max-y_min;
01755 if (rw<=0||rh<=0)
01756 return false;
01757 chp.resize(rw, rh);
01758 for (int y = y_min; y<y_max; y++)
01759 for (int x =x_min; x<x_max; x++)
01760 chp(x-x_min, y-y_min) = input(x, y);
01761 return true;
01762 }
01763
01764
01765 bool brip_vil1_float_ops::chip(vil1_image const & input,
01766 brip_roi_sptr const& roi,
01767 vil1_image& chip)
01768 {
01769 if (!input||!roi)
01770 return false;
01771 const int Nc = input.width(), Nr = input.height();
01772 int c_min = roi->cmin(0), r_min = roi->rmin(0);
01773 int c_max = roi->cmax(0), r_max = roi->rmax(0);
01774 if (c_min<0)
01775 c_min = 0;
01776 if (r_min<0)
01777 r_min = 0;
01778 if (c_max>Nc-1)
01779 c_max=Nc-1;
01780 if (r_max>Nr-1)
01781 r_max=Nc-1;
01782 int CNc = c_max-c_min, CNr = r_max-r_min;
01783 if (CNc<=0||CNr<=0)
01784 return false;
01785
01786 if (input.components()==3)
01787 {
01788 vil1_memory_image_of<vil1_rgb<unsigned char> > timage(input);
01789 vil1_memory_image_of<vil1_rgb<unsigned char> > tchip(CNc, CNr);
01790 for (int r = r_min; r<r_max; r++)
01791 for (int c =c_min; c<c_max; c++)
01792 tchip(c-c_min, r-r_min) = timage(c, r);
01793 chip = tchip;
01794 return true;
01795 }
01796
01797 if (input.component_format()==VIL1_COMPONENT_FORMAT_IEEE_FLOAT)
01798 {
01799 vil1_memory_image_of<float> timage(input);
01800 vil1_memory_image_of<float> tchip(CNc, CNr);
01801 for (int r = r_min; r<r_max; r++)
01802 for (int c =c_min; c<c_max; c++)
01803 tchip(c-c_min, r-r_min) = timage(c, r);
01804 chip = tchip;
01805 return true;
01806 }
01807
01808 if (input.get_size_bytes() ==2)
01809 {
01810 vil1_memory_image_of<unsigned short> timage(input);
01811 vil1_memory_image_of<unsigned short> tchip(CNc, CNr);
01812 for (int r = r_min; r<r_max; r++)
01813 for (int c =c_min; c<c_max; c++)
01814 tchip(c-c_min, r-r_min) = timage(c, r);
01815 chip = tchip;
01816 return true;
01817 }
01818
01819
01820 if (true)
01821 {
01822 vil1_memory_image_of<unsigned char> timage(input);
01823 vil1_memory_image_of<unsigned char> tchip(CNc, CNr);
01824 for (int r = r_min; r<r_max; r++)
01825 for (int c =c_min; c<c_max; c++)
01826 tchip(c-c_min, r-r_min) = timage(c, r);
01827 chip = tchip;
01828 return true;
01829 }
01830
01831 return false;
01832 }
01833
01834
01835
01836 vil1_image brip_vil1_float_ops::insert_chip_in_image(vil1_image const & image,
01837 vil1_image const & chip,
01838 brip_roi_sptr const& roi)
01839 {
01840 if (!chip||!roi)
01841 return image;
01842
01843 vil1_image temp(image);
01844 const int chip_cols = chip.width(), chip_rows = chip.height();
01845
01846
01847 if (image.components()==3)
01848 {
01849 vil1_memory_image_of<vil1_rgb<unsigned char> > timage(image);
01850 vil1_memory_image_of<vil1_rgb<unsigned char> > tchip(chip);
01851 for (int cr = 0; cr<chip_rows; cr++)
01852 for (int cc = 0; cc<chip_cols; cc++)
01853 {
01854 int imgc = roi->ic(cc), imgr = roi->ir(cr);
01855 timage(imgc, imgr) = tchip(cc, cr);
01856 }
01857 return timage;
01858 }
01859 return image;
01860 }
01861
01862
01863 static float cross_corr(const double area, const double si1, const double si2,
01864 const double si1i1,
01865 const double si2i2, const double si1i2,
01866 const float intensity_thresh)
01867 {
01868 if (!area)
01869 return 0.f;
01870
01871 double u1 = si1/area, u2 = si2/area;
01872 if (u1<intensity_thresh||u2<intensity_thresh)
01873 return -1.f;
01874 double neu = si1i2 - area*u1*u2;
01875 double sd1 = vcl_sqrt(si1i1-area*u1*u1), sd2 = vcl_sqrt(si2i2-area*u2*u2);
01876 if (!neu)
01877 return 0.f;
01878 if (!sd1||!sd2) {
01879 if (neu>0)
01880 return 1.f;
01881 else
01882 return -1.f;
01883 }
01884 double den = sd1*sd2;
01885 return float(neu/den);
01886 }
01887
01888
01889
01890 float brip_vil1_float_ops::
01891 cross_correlate(vil1_memory_image_of<float> const & image1,
01892 vil1_memory_image_of<float> const & image2,
01893 const float x, const float y,
01894 const int radius,
01895 const float intensity_thresh)
01896 {
01897 const int w1 = image1.width(), h1 = image1.height();
01898 const int w2 = image1.width(), h2 = image1.height();
01899
01900 if (w1!=w2||h1!=h2)
01901 return -1;
01902 if (x<radius||x>w1-radius-1||y<radius||y>h1-radius-1)
01903 return -1;
01904
01905
01906
01907 double sI1=0, sI2=0, sI1I1=0, sI2I2=0, sI1I2=0;
01908 for (int y0 = -10*radius; y0<=10*radius; ++y0)
01909 for (int x0 = -10*radius; x0<=10*radius; ++x0)
01910 {
01911 float xp = x+0.1f*x0, yp = y+0.1f*y0;
01912 double v1 = brip_vil1_float_ops::bilinear_interpolation(image1, xp, yp);
01913 double v2 = brip_vil1_float_ops::bilinear_interpolation(image2, xp, yp);
01914 sI1 += v1;
01915 sI2 += v2;
01916 sI1I1 += v1*v1;
01917 sI2I2 += v2*v2;
01918 sI1I2 += v1*v2;
01919 }
01920
01921 int s = 2*radius+1;
01922 double area = s*s;
01923 return cross_corr(area, sI1, sI2, sI1I1, sI2I2, sI1I2, intensity_thresh);
01924 }
01925
01926
01927
01928 static bool update_row(vil1_memory_image_of<float> const& image1,
01929 vil1_memory_image_of<float> const& image2,
01930 const int r0,
01931 const int r,
01932 vbl_array_2d<double>& SI1,
01933 vbl_array_2d<double>& SI2,
01934 vbl_array_2d<double>& SI1I1,
01935 vbl_array_2d<double>& SI2I2,
01936 vbl_array_2d<double>& SI1I2)
01937 {
01938 const int w1 = image1.width(), h1 = image1.height();
01939 const int w2 = image2.width(), h2 = image2.height();
01940 if (w1!=w2||h1!=h2||r<0||r>=h1)
01941 return false;
01942 double i10 = image1(0,r0), i20 = image2(0,r0);
01943 SI1[r][0] = i10; SI2[r][0] = i20; SI1I1[r][0]=i10*i10;
01944 SI2I2[r][0]=i20*i20; SI1I2[r][0]=i10*i20;
01945 for (int c = 1; c<w1; c++)
01946 {
01947 double i1c = image1(c,r0);
01948 double i2c = image2(c,r0);
01949 SI1[r][c] = SI1[r][c-1]+i1c;
01950 SI2[r][c] = SI2[r][c-1]+i2c;
01951 SI1I1[r][c] = SI1I1[r][c-1]+ i1c*i1c;
01952 SI2I2[r][c] = SI2I2[r][c-1]+ i2c*i2c;
01953 SI1I2[r][c] = SI1I2[r][c-1]+ i1c*i2c;
01954 }
01955 return true;
01956 }
01957
01958 static bool initialize_slice(vil1_memory_image_of<float> const& image1,
01959 vil1_memory_image_of<float> const& image2,
01960 const int radius,
01961 vbl_array_2d<double>& SI1,
01962 vbl_array_2d<double>& SI2,
01963 vbl_array_2d<double>& SI1I1,
01964 vbl_array_2d<double>& SI2I2,
01965 vbl_array_2d<double>& SI1I2)
01966 {
01967 for (int r = 0; r<=2*radius; r++)
01968 if (!update_row(image1, image2, r, r, SI1, SI2, SI1I1, SI2I2, SI1I2))
01969 return false;
01970 return true;
01971 }
01972
01973 static bool collapse_slice( vbl_array_2d<double> const& SI1,
01974 vbl_array_2d<double> const& SI2,
01975 vbl_array_2d<double> const& SI1I1,
01976 vbl_array_2d<double> const& SI2I2,
01977 vbl_array_2d<double> const& SI1I2,
01978 vbl_array_1d<double>& dSI1,
01979 vbl_array_1d<double>& dSI2,
01980 vbl_array_1d<double>& dSI1I1,
01981 vbl_array_1d<double>& dSI2I2,
01982 vbl_array_1d<double>& dSI1I2)
01983 {
01984
01985 int w = SI1.cols(), h = SI1.rows();
01986 int dw = SI1.cols();
01987 if (dw!=w)
01988 return false;
01989
01990 for (int c = 0; c<w; c++)
01991 {
01992 dSI1[c]=0; dSI2[c]=0; dSI1I1[c]=0;
01993 dSI2I2[c]=0; dSI1I2[c]=0;
01994 for (int r = 0; r<h; r++)
01995 {
01996 dSI1[c] += SI1[r][c];
01997 dSI2[c] += SI2[r][c];
01998 dSI1I1[c] += SI1I1[r][c];
01999 dSI2I2[c] += SI2I2[r][c];
02000 dSI1I2[c] += SI1I2[r][c];
02001 }
02002 }
02003 return true;
02004 }
02005
02006 static bool cross_correlate_row(int radius,
02007 vbl_array_1d<double>& dSI1,
02008 vbl_array_1d<double>& dSI2,
02009 vbl_array_1d<double>& dSI1I1,
02010 vbl_array_1d<double>& dSI2I2,
02011 vbl_array_1d<double>& dSI1I2,
02012 float intensity_thresh,
02013 vbl_array_1d<float>& cc
02014 )
02015 {
02016
02017 int w = dSI1.size(), wc = cc.size();
02018 if (!w||!wc||w!=wc)
02019 return false;
02020 int s = 2*radius+1;
02021 double area = s*s;
02022
02023 double si1=dSI1[s-1], si2=dSI2[s-1], si1i1=dSI1I1[s-1],
02024 si2i2=dSI2I2[s-1], si1i2=dSI1I2[s-1];
02025 cc[radius]= cross_corr(area, si1, si2, si1i1, si2i2, si1i2, intensity_thresh);
02026
02027 for (int c = radius+1; c+radius<w; c++)
02028 {
02029 si1=dSI1[c+radius]-dSI1[c-radius-1];
02030 si2=dSI2[c+radius]-dSI2[c-radius-1];
02031 si1i1=dSI1I1[c+radius]-dSI1I1[c-radius-1];
02032 si2i2=dSI2I2[c+radius]-dSI2I2[c-radius-1];
02033 si1i2=dSI1I2[c+radius]-dSI1I2[c-radius-1];
02034 cc[c] = cross_corr(area, si1, si2, si1i1, si2i2, si1i2, intensity_thresh);
02035 }
02036
02037 return true;
02038 }
02039
02040 static void advance_rows(vbl_array_2d<double>& S)
02041 {
02042 int nr = S.rows(), nc = S.cols();
02043 for (int r = 0; r<nr-1; r++)
02044 for (int c =0; c<nc; c++)
02045 S[r][c]=S[r+1][c];
02046 }
02047
02048
02049 static bool output_cc_row(const int r0, vbl_array_1d<float> const& cc,
02050 vil1_memory_image_of<float>& out)
02051 {
02052 const int n = cc.size(), w = out.width();
02053 if (n!=w)
02054 return false;
02055 for (int c = 0; c<w; c++)
02056 out(c, r0) = cc[c];
02057 return true;
02058 }
02059
02060
02061 bool brip_vil1_float_ops::
02062 cross_correlate(vil1_memory_image_of<float> const & image1,
02063 vil1_memory_image_of<float> const & image2,
02064 vil1_memory_image_of<float>& out,
02065 const int radius,
02066 const float intensity_thresh)
02067 {
02068 vul_timer t;
02069 const int w = image1.width(), h = image1.height();
02070 const int w2 = image2.width(), h2 = image2.height();
02071
02072 if (w!=w2||h!=h2)
02073 {
02074 vcl_cout << "In brip_vil1_float_ops::fast_cross_correlate(..) -"
02075 << " image sizes don't match\n";
02076 return false;
02077 }
02078 out.resize(w, h);
02079 out.fill(0.f);
02080 int s = 2*radius+1;
02081
02082 vbl_array_2d<double> SI1(s,w), SI2(s,w),
02083 SI1I1(s,w), SI2I2(s,w), SI1I2(s,w);
02084 vbl_array_1d<float> cc(w, 0.f);
02085 vbl_array_1d<double> dSI1(w, 0.0), dSI2(w, 0.0),
02086 dSI1I1(w, 0.0), dSI2I2(w, 0.0), dSI1I2(w, 0.0);
02087 initialize_slice(image1, image2, radius, SI1, SI2, SI1I1, SI2I2, SI1I2);
02088 if (!collapse_slice(SI1, SI2, SI1I1, SI2I2, SI1I2,
02089 dSI1, dSI2, dSI1I1, dSI2I2, dSI1I2))
02090 return false;
02091 int r0 = radius;
02092 for (; r0+radius+1<h; r0++)
02093 {
02094 if (r0==5)
02095 r0=5;
02096 #ifdef DEBUG
02097 vcl_cout << "r0 " << r0 << '\n';
02098 #endif
02099 if (!cross_correlate_row(radius, dSI1, dSI2, dSI1I1, dSI2I2, dSI1I2,
02100 intensity_thresh, cc))
02101 return false;
02102 #ifdef DEBUG
02103 vcl_cout << '\n';
02104 #endif
02105 advance_rows(SI1); advance_rows(SI2); advance_rows(SI1I1);
02106 advance_rows(SI2I2); advance_rows(SI1I2);
02107 if (!update_row(image1, image2, r0+radius+1, 2*radius,
02108 SI1, SI2, SI1I1, SI2I2, SI1I2))
02109 return out;
02110 if (!collapse_slice(SI1, SI2, SI1I1, SI2I2, SI1I2,
02111 dSI1, dSI2, dSI1I1, dSI2I2, dSI1I2))
02112 return false;
02113 if (!output_cc_row(r0, cc, out))
02114 return false;
02115 }
02116
02117 #ifdef DEBUG
02118 vcl_cout << "r0 " << r0 << '\n';
02119 #endif
02120 if (!cross_correlate_row(radius, dSI1, dSI2, dSI1I1, dSI2I2, dSI1I2,
02121 intensity_thresh, cc))
02122 return false;
02123 #ifdef DEBUG
02124 vcl_cout << '\n';
02125 #endif
02126 if (!output_cc_row(r0, cc, out))
02127 return false;
02128 vcl_cout << "RunningSumCrossCorrelation for " << w*h/1000.0f << " k pixels in "
02129 << t.real() << " msecs\n"<< vcl_flush;
02130 return true;
02131 }