contrib/brl/bseg/brip/brip_vil1_float_ops.cxx
Go to the documentation of this file.
00001 #include "brip_vil1_float_ops.h"
00002 //:
00003 // \file
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 //:  Convolve with a kernel
00023 //   It's assumed that the kernel is square with odd dimensions
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(); // kh = kernel.rows();
00030   // add a check for kernels that are not equal dimensions of odd size JLM
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 //: Downsamples the 1-d array by 2 using the Burt-Adelson reduction algorithm.
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]);//reflect at boundary
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     //shift the window, w, over by two pixels
00074     w[0] = w[2];       w[1] = w[3];     w[2] = w[4];
00075     //handle the boundary conditions
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 //: Downsamples the image by 2 using the Burt-Adelson reduction algorithm.
00084 // Convolution with a 5-point kernel [(0.5-ka)/2, 0.25, ka, 0.25, (0.5-ka)/2]
00085 // ka = 0.6  maximum decorrelation, wavelet for image compression.
00086 // ka = 0.5  linear interpolation,
00087 // ka = 0.4  Gaussian filter
00088 // ka = 0.359375 min aliasing, wider than Gaussian
00089 // The image sizes are related by: output_dimension = (input_dimension +1)/2.
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   //Generate input/output arrays
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   //Initialize arrays
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   //downsample initial arrays
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   //do the first output line
00122   for (;x<half_w;x++)
00123     output(x,0)= k0*out0[x]+ 2.0f*(k1*out1[x]+k2*out2[x]);
00124   //normal lines
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     //shift the neighborhood down two lines
00130     float* temp0 = out0;
00131     float* temp1 = out1;
00132     out0 = out2;  out1 = out3;  out2 = out4;
00133     out3 = temp0; out4 = temp1;//reflect values
00134     //test border condition
00135     if (y<half_h-2)
00136     {
00137       //normal processing, so don't reflect
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   //make the three color planes
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 // Determine if the center of a (2n+1)x(2n+1) neighborhood is a local maximum
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 // Interpolate the sub-pixel position of a neighborhood using a
00227 // second order expansion on a 3x3 sub-neighborhood. Return the
00228 // offset to the maximum, i.e. x=x0+dx, y = y0+dy. The design is
00229 // similar to the droid counterpoint by fsm, which uses the Beaudet Hessian
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   //extract the neighborhood
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   //Compute the 2nd order quadratic coefficients
00248   //      1/6 * [ -1  0 +1 ]
00249   // Ix =       [ -1  0 +1 ]
00250   //            [ -1  0 +1 ]
00251   float Ix =(-n_m1_m1+n_m1_1-n_0_m1+n_0_1-n_1_m1+n_1_1)/6.0f;
00252   //      1/6 * [ -1 -1 -1 ]
00253   // Iy =       [  0  0  0 ]
00254   //            [ +1 +1 +1 ]
00255   float Iy =(-n_m1_m1-n_m1_0-n_m1_1+n_1_m1+n_1_0+n_1_1)/6.0f;
00256   //      1/3 * [ +1 -2 +1 ]
00257   // Ixx =      [ +1 -2 +1 ]
00258   //            [ +1 -2 +1 ]
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   //      1/4 * [ +1  0 -1 ]
00262   // Ixy =      [  0  0  0 ]
00263   //            [ -1  0 +1 ]
00264   float Ixy = (n_m1_m1-n_m1_1+n_1_m1+n_1_1)/4.0f;
00265   //      1/3 * [ +1 +1 +1 ]
00266   // Iyy =      [ -2 -2 -2 ]
00267   //            [ +1 +1 +1 ]
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   // The next bit is to find the extremum of the fitted surface by setting its
00272   // partial derivatives to zero. We need to solve the following linear system :
00273   // Given the fitted surface is
00274   // I(x,y) = Io + Ix x + Iy y + 1/2 Ixx x^2 + Ixy x y + 1/2 Iyy y^2
00275   // we solve for the maximum (x,y),
00276   //
00277   //  [ Ixx Ixy ] [ dx ] + [ Ix ] = [ 0 ]      (dI/dx = 0)
00278   //  [ Ixy Iyy ] [ dy ]   [ Iy ]   [ 0 ]      (dI/dy = 0)
00279   //
00280   float det = Ixx*Iyy - Ixy*Ixy;
00281   // det>0 corresponds to a true local extremum otherwise a saddle point
00282   if (det>0)
00283   {
00284     dx = (Iy*Ixy - Ix*Iyy) / det;
00285     dy = (Ix*Ixy - Iy*Ixx) / det;
00286     // more than one pixel away
00287     if (vcl_fabs(dx) > 1.0 || vcl_fabs(dy) > 1.0)
00288       dx = 0; dy = 0;
00289   }
00290 }
00291 
00292 //---------------------------------------------------------------
00293 // Compute the local maxima of the input on a (2n+1)x(2n+2)
00294 // neighborhood above the given threshold. At each local maximum,
00295 // compute the sub-pixel location, (x_pos, y_pos).
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       //If the center is not above threshold then there is
00313       //no hope
00314       if (input(x,y)<thresh)
00315         continue;
00316       //Fill the neighborhood
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       //Check if the center is a local maximum
00321       float dx, dy, max_v;
00322       if (brip_vil1_float_ops::local_maximum(neighborhood, n, max_v))
00323       {
00324         //if so sub-pixel interpolate (3x3) and output results
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 // Subtract image_1 from image_2.
00336 // Will not operate unless the two input images are the same dimensions
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 // Compute the gradient of the input, use a 3x3 mask
00378 //
00379 //         1  |-1  0  1|         1  |-1 -1 -1|
00380 //   Ix = --- |-1  0  1|   Iy = --- | 0  0  0|
00381 //         6  |-1  0  1|         6  | 1  1  1|
00382 //
00383 // Larger masks are computed by pre-convolving with a Gaussian
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 // Compute the Hessian of the input, use a 3x3 mask
00413 //
00414 //          1 | 1  -2  1|          1 |  1  1  1|         1  | 1  0 -1|
00415 //   Ixx = ---| 1  -2  1|   Iyy = ---| -2 -2 -2|  Ixy = --- | 0  0  0|
00416 //          3 | 1  -2  1|          3 |  1  1  1|         4  |-1  0  1|
00417 //
00418 // Larger masks are computed by pre-convolving with a Gaussian
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       //compute eigenvalues for experimentation
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; //just det for now
00477     }
00478   return output;
00479 }
00480 
00481 //----------------------------------------------------------------
00482 //   t
00483 //IxIx gradient matrix elements
00484 // That is,
00485 //                        _                           _
00486 //                       | (dI/dx)^2    (dI/dx)(dI/dy) |
00487 //                       |                             |
00488 //  A = Sum(neighborhood)|                             |
00489 //                       |(dI/dx)(dI/dy)   (dI/dx)^2   |
00490 //                       |_                           _|
00491 //
00492 // over a 2n+1 x 2n+1 neighborhood
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; // Scale the output to values in the 10->1000 range
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 // Compute the sqrt of the product of the eigenvalues of the
00556 // gradient matrix over a 2n+1 x 2n+1 neighborhood
00557 // That is,
00558 //                        _                           _
00559 //                       | (dI/dx)^2    (dI/dx)(dI/dy) |
00560 //                       |                             |
00561 //  A = Sum(neighborhood)|                             |
00562 //                       |(dI/dx)(dI/dy)   (dI/dx)^2   |
00563 //                       |_                           _|
00564 //
00565 //  The output image is sqrt(lamba_1*lambda_2) where lambda_i are the
00566 //  eigenvalues
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 // Lucas-Kanade motion vectors:  Solve for the motion vectors over a
00603 // (2n+1)x(2n+1) neighborhood. The time derivative of intensity is computed
00604 // from the previous_frame. The threshold eliminates small values of
00605 // the product of the time derivative and the motion matrix eigenvalues,
00606 // i.e, |lambda_1*lambda_2*dI/dt|<thresh.  Thus motion is only reported when
00607 // the solution is well-conditioned.
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   //compute the gradient vector and the time derivative
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   //sum the motion terms over the (2n+1)x(2n+1) neighborhood.
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       //Divide by the number of pixels in the neighborhood
00642       IxIx/=N;  IxIy/=N; IyIy/=N; IxIt/=N; IyIt/=N;
00643       float det = float(IxIx*IyIy-IxIy*IxIy);
00644       //Eliminate small motion factors
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       //solve for the motion vector
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   //determine the max min values
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 // Convert the range between min_val and max_val to 255
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   // Reference: figure 13.36, page 595 of Foley & van Dam
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   //lightness
00851   float la = (maxval + minval) / 2.f;
00852   // Achromatic case, intensity is grey or near black or white
00853   if (maxval == minval||i<20||i>235)
00854   {
00855     s = -1.f;
00856     h = 0.f;
00857   }
00858   else//the chromatic case
00859   {
00860     // Calculate the saturation.
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     // Calculate the hue.
00875     float delta = maxval - minval;
00876     if (r == maxval)
00877     {
00878       // The resulting color is between yellow and magenta.
00879       h = (60*(g - b))/ delta;
00880     }
00881     else if (g == maxval)
00882     {
00883       // The resulting color is between cyan and yellow.
00884       h = 120.f + (60*(b - r))/delta;
00885     }
00886     else
00887     {
00888       // The resulting color is between magenta and cyan.
00889       h = 240.f + (60*(r - g))/delta;
00890     }
00891     // Be sure 0 <= hue <= 360
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 //: map so that intensity is proportional to saturation and hue is color
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     //already a float image
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 // : convert a vil1_rgb<unsigned char> image to an unsigned_char image.
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   //Check if the image is a float
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   //Here we assume that the image is an unsigned char
01039   //In this case we should just return it.
01040   if (image.components()!=3)
01041     return vil1_memory_image_of<unsigned char>(image);
01042 
01043   // the image is color so we should convert it to greyscale
01044   // Here we assume the color elements are unsigned char.
01045   vil1_memory_image_of<vil1_rgb<unsigned char> > color_image(image);
01046   const int width = color_image.width(), height = color_image.height();
01047   // the output image
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 // Read a convolution kernel from file
01058 // Assumes a square kernel with odd dimensions, i.e., w,h = 2n+1
01059 // format:
01060 //     n
01061 //     scale
01062 //     k00  k01  ... k02n
01063 //           ...
01064 //     k2n0 k2n1 ... k2n2n
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) // row runs from 0 to width*height-1
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   //Insert the images into matrix I
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   //Compute the SVD of matrix I
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   //Extract the Basis images
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   //Output the basis images
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 //: 1d fourier transform
01169 //-------------------------------------------------------------------------
01170 // This computes an in-place complex-to-complex FFT
01171 // x and y are the real and imaginary arrays of 2^m points.
01172 // dir =  1 gives forward transform
01173 // dir = -1 gives reverse transform
01174 //
01175 //   Formula: forward
01176 //                N-1
01177 //                ---
01178 //            1   \          - j k 2 pi n / N
01179 //    X(n) = ---   >   x(k) e                    = forward transform
01180 //            N   /                                n=0..N-1
01181 //                ---
01182 //                k=0
01183 //
01184 //    Formula: reverse
01185 //                N-1
01186 //                ---
01187 //                \          j k 2 pi n / N
01188 //    X(n) =       >   x(k) e                    = forward transform
01189 //                /                                n=0..N-1
01190 //                ---
01191 //                k=0
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   /* Calculate the number of points */
01199   nn = 1;
01200   for (i=0;i<m;i++)
01201     nn *= 2;
01202 
01203   /* Do the bit reversal */
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   // Compute the FFT
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   // Scaling for forward transform
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 //  Perform a 2D FFT inplace given a complex 2D array
01265 //  The direction dir, 1 for forward, -1 for reverse
01266 //  The size of the array (nx,ny)
01267 //  Return false if there are memory problems or
01268 //  the dimensions are not powers of 2
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   /* Transform the rows */
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   /* Transform the columns */
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 //: reorder the transform values to sequential frequencies as in conventional Fourier transforms.
01319 //  The transformation is its self-inverse.
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 //:  Compute the fourier transform.
01345 //   If the image dimensions are not a power of 2 then the operation fails.
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   //fill the fft matrix
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   //extract magnitude and phase
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;//pad with zeroes
01429 }
01430 
01431 //: resize the input to the closest power of two image dimensions
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; //we wouldn't want to have such large images in memory
01437   const int w = input.width(), h = input.height();
01438   int prodw = 1, prodh = 1;
01439   //Find power of two width
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   //Find power of two height
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 //: block a periodic signal by suppressing two Gaussian lobes in the frequency domain.
01463 //  The lobes are on the line defined by dir_fx and dir_fy through the
01464 //  dc origin, assumed (0, 0).  The center frequency, f0, is the distance along
01465 //  the line to the center of each blocking lobe (+- f0). radius is the
01466 //  standard deviation of each lobe. Later we can define a "filter" class.
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   // normalize dir_fx and dir_fy
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   // compute the centers of each lobe
01482   float fx0p = dx*f0, fy0p = dy*f0;
01483   float fx0m = -dx*f0, fy0m = -dy*f0;
01484   // the squared distance of fx, fy from each center
01485   float d2p = (fx-fx0p)*(fx-fx0p) + (fy-fy0p)*(fy-fy0p);
01486   float d2m = (fx-fx0m)*(fx-fx0m) + (fy-fy0m)*(fy-fy0m);
01487   // use the closest lobe
01488   float d = d2p;
01489   if (d2m<d2p)
01490     d = d2m;
01491   // the gaussian blocking function
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   //Compute the fourier transform of the image.
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   //filter the magnitude function
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   //Transform back
01528   pow_two_filt.resize(Nfx, Nfy);
01529   brip_vil1_float_ops::inverse_fourier_transform(bmag, phase, pow_two_filt);
01530 
01531   //Resize to original input size
01532   brip_vil1_float_ops::resize(pow_two_filt, input.width(), input.height(), output);
01533   return true;
01534 }
01535 
01536 //----------------------------------------------------------------------
01537 //: Bi-linear interpolation on the neighborhood below.
01538 //      xr
01539 //   yr 0  x
01540 //      x  x
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   //check bounds
01547   const int w = input.width(), h = input.height();
01548   //the pixel containing the interpolated point
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 //: Transform the input to the output by a homography.
01564 //  if the output size is fixed then only the corresponding
01565 //  region of input image space is transformed.
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   //First, there is some rather complex bookeeping to insure that
01576   //the input and output image rois are consistent with the homography.
01577 
01578   // the bounding boxes corresponding to input and output rois
01579   // We also construct polygons since homographies turn boxes into arbitrary
01580   // quadrilaterals.
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   // set up the roi and poly for the input image
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   //Case I
01591   // the output image size and input transform can be adjusted
01592   // to map the transformed image onto the full range
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     //offset the transform and transformed roi so that lower left is (0,0)
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 // Case II, the output image size is fixed so we have to find the
01610   {  // inverse mapping of the output roi and intersect with the input roi
01611     //  to determine the domain of the mapping
01612     if (!output)
01613       return false;
01614     //The output roi and poly
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     //Construct the reverse mapping of the output bounds
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     //form the roi corresponding to the inverse mapped output bounds
01629     vsol_box_2d_sptr tbox = tpoly->get_bounding_box();
01630 
01631     //intersect with the input image bounds to get the input roi
01632     vsol_box_2d_sptr temp;
01633     if (!bsol_algs::intersection(tbox, input_roi, temp))
01634       return false;
01635     input_roi = temp;
01636   }
01637   //At this point we have the correct bounds for the input and
01638   //the output image
01639 
01640   //Iterate over the output image space and map the location of each
01641   //pixel into the input image space. Then carry out interpolation to
01642   //get the value of each output pixel
01643 
01644   // Dimensions of the input image
01645   int ailow  = int(input_roi->get_min_x()+0.9999f); // round up to nearest int
01646   int aihigh = int(input_roi->get_max_x());      // round down to nearest int
01647   int ajlow  = int(input_roi->get_min_y()+0.9999f);
01648   int ajhigh = int(input_roi->get_max_y());
01649 
01650   // Dimensions of the output image
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   /* The inverse transform is used to map backwards from the output */
01657   const vnl_matrix_fixed<double,3,3>& Minv = Hinv.get_matrix();
01658 
01659   /* Now use Hinv to transform the image */
01660   for (int i = bilow; i<bihigh; i++)
01661     for (int j = bjlow; j<bjhigh; j++)
01662     {
01663       /* Transform the pixel */
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       /* Now do linear interpolation */
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           /* Get the neighbouring pixels */
01684           /*      (u  v)    (u+1  v)     */
01685           /*      (u v+1)   (u+1 v+1)    */
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         /* Set the value */
01697         output(i,j) = val;
01698       }
01699     }
01700   return true;
01701 }
01702 
01703 //: rotate the input image counter-clockwise about the image origin.
01704 // demonstrates the use of image homography
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   //map theta_deg to [0 360]
01714   while (ang>360)
01715     ang-=360;
01716   while (ang<0)
01717     ang+=360;
01718   //convert to radians
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   //counter clockwise rotation about the image origin (0, 0)
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   //The transform is adjusted to map the full input domain onto
01730   //the output image.
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 //: Chipping for a general image type
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   //BAD but for now force to byte output regardless FIX FIX FIX!
01819   //if (input.get_size_bytes() ==1)
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 //:assumes that the chip and image have the same pixel types.  Only works for
01835 // color at present.
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   //copy the input
01843   vil1_image temp(image);
01844   const int chip_cols = chip.width(), chip_rows = chip.height();
01845   //need to do cases
01846   //but just color now
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;//no op
01860 }
01861 
01862 //:compute normalized cross correlation from the intensity moment sums.
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   //the mean values
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 //:perform normalized cross-correlation at a sub-pixel location
01889 // thus all the pixel values are interpolated.
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   //bounds checks
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   //accumulate correlation sums,
01906   //bi-linear interpolate the values
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   //:compute correlation.
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 //: r0 is the image from which to read the new intensity values
01927 //  r is the summing array row in which the values are to be accumulated
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   //sanity check
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   //sanity check
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   //the general case
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   //the remaining columns
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   //sizes must match
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   //Create the running sum slices
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   //handle the last row
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 }