core/vnl/vnl_fastops.cxx
Go to the documentation of this file.
00001 // This is core/vnl/vnl_fastops.cxx
00002 #ifdef VCL_NEEDS_PRAGMA_INTERFACE
00003 #pragma implementation
00004 #endif
00005 //:
00006 // \file
00007 // \author Andrew W. Fitzgibbon, Oxford RRG
00008 // \date   08 Dec 96
00009 //
00010 //-----------------------------------------------------------------------------
00011 
00012 #include "vnl_fastops.h"
00013 
00014 #include <vcl_cstdlib.h> // abort()
00015 #include <vcl_cstring.h> // memset()
00016 #include <vcl_iostream.h>
00017 
00018 //: Compute $A^\top A$.
00019 void vnl_fastops::AtA(vnl_matrix<double>& out, const vnl_matrix<double>& A)
00020 {
00021   const unsigned int n = A.columns();
00022   // Verify output is the right size
00023   if (out.rows() != n || out.columns() != n)
00024     out.set_size(n,n);
00025 
00026   const unsigned int m = A.rows();
00027 
00028   double const* const* a = A.data_array();
00029   double** ata = out.data_array();
00030 
00031 #if 0
00032     for (unsigned int i = 0; i < n; ++i)
00033       for (unsigned int j = i; j < n; ++j) {
00034         double accum = 0;
00035         for (unsigned int k = 0; k < m; ++k)
00036           accum += a[k][i] * a[k][j];
00037         ata[i][j] = ata[j][i] = accum;
00038       }
00039 #else // 5 times faster on 600 Mhz Pentium III for m = 10000, n = 50
00040     vcl_memset(ata[0], 0, n * n * sizeof ata[0][0]);
00041     for (unsigned int k = 0; k < m; ++k)
00042       for (unsigned int i = 0; i < n; ++i) {
00043         double aki = a[k][i];
00044         double const* arow = a[k] + i;
00045         double* atarow = ata[i] + i;
00046         double const* arowend = a[k] + n;
00047         while (arow != arowend)
00048           *atarow++ += aki * *arow++;
00049       }
00050       for (unsigned int i = 0; i < n; ++i)
00051         for (unsigned int j = i+1; j < n; ++j)
00052           ata[j][i] = ata[i][j];
00053 #endif // 0
00054 }
00055 
00056 //: Compute AxB.
00057 void vnl_fastops::AB(vnl_matrix<double>& out, const vnl_matrix<double>& A, const vnl_matrix<double>& B)
00058 {
00059   const unsigned int na = A.columns();
00060   const unsigned int mb = B.rows();
00061 
00062   // Verify matrices compatible
00063   if (na != mb) {
00064     vcl_cerr << "vnl_fastops::AB: argument sizes do not match: " << na << " != " << mb << '\n';
00065     vcl_abort();
00066   }
00067 
00068   const unsigned int ma = A.rows();
00069   const unsigned int nb = B.columns();
00070 
00071   // Verify output is the right size
00072   if (out.rows() != ma || out.columns() != nb)
00073     out.set_size(ma,nb);
00074 
00075   double const* const* a = A.data_array();
00076   double const* const* b = B.data_array();
00077   double** outdata = out.data_array();
00078 
00079   for (unsigned int i = 0; i < ma; ++i)
00080     for (unsigned int j = 0; j < nb; ++j) {
00081       double accum = 0;
00082       for (unsigned int k = 0; k < na; ++k)
00083         accum += a[i][k] * b[k][j];
00084       outdata[i][j] = accum;
00085     }
00086 }
00087 
00088 //: Compute $A^\top B$.
00089 void vnl_fastops::AtB(vnl_matrix<double>& out, const vnl_matrix<double>& A, const vnl_matrix<double>& B)
00090 {
00091   const unsigned int ma = A.rows();
00092   const unsigned int mb = B.rows();
00093 
00094   // Verify matrices compatible
00095   if (ma != mb) {
00096     vcl_cerr << "vnl_fastops::AtB: argument sizes do not match: " << ma << " != " << mb << '\n';
00097     vcl_abort();
00098   }
00099 
00100   const unsigned int na = A.columns();
00101   const unsigned int nb = B.columns();
00102 
00103   // Verify output is the right size
00104   if (out.rows() != na || out.columns() != nb)
00105     out.set_size(na,nb);
00106 
00107   double const* const* a = A.data_array();
00108   double const* const* b = B.data_array();
00109   double** outdata = out.data_array();
00110 
00111   for (unsigned int i = 0; i < na; ++i)
00112     for (unsigned int j = 0; j < nb; ++j) {
00113       double accum = 0;
00114       for (unsigned int k = 0; k < ma; ++k)
00115         accum += a[k][i] * b[k][j];
00116       outdata[i][j] = accum;
00117     }
00118 }
00119 
00120 //: Compute $A^\top b$ for vector b. out may not be b.
00121 void vnl_fastops::AtB(vnl_vector<double>& out, const vnl_matrix<double>& A, const vnl_vector<double>& B)
00122 {
00123   const unsigned int m = A.rows();
00124   const unsigned int l = B.size();
00125 
00126   // Verify matrices compatible
00127   if (m != l) {
00128     vcl_cerr << "vnl_fastops::AtB: argument sizes do not match: " << m << " != " << l << '\n';
00129     vcl_abort();
00130   }
00131 
00132   const unsigned int n = A.columns();
00133 
00134   // Verify output is the right size
00135   if (out.size() != n)
00136     out.set_size(n);
00137 
00138   double const* const* a = A.data_array();
00139   double const* b = B.data_block();
00140   double* outdata = out.data_block();
00141 
00142   for (unsigned int i = 0; i < n; ++i) {
00143     double accum = 0;
00144     for (unsigned int k = 0; k < l; ++k)
00145       accum += a[k][i] * b[k];
00146    outdata[i] = accum;
00147   }
00148 }
00149 
00150 //: Compute $A b$ for vector b. out may not be b.
00151 void vnl_fastops::Ab(vnl_vector<double>& out, const vnl_matrix<double>& A, const vnl_vector<double>& b)
00152 {
00153   const unsigned int m = A.cols();
00154   const unsigned int l = b.size();
00155 
00156   // Verify matrices compatible
00157   if (m != l) {
00158     vcl_cerr << "vnl_fastops::Ab: argument sizes do not match: " << m << " != " << l << '\n';
00159     vcl_abort();
00160   }
00161 
00162   const unsigned int n = A.rows();
00163 
00164   // Verify output is the right size
00165   if (out.size() != n)
00166     out.set_size(n);
00167 
00168   double const* const* a = A.data_array();
00169   double const* bb = b.data_block();
00170   double* outdata = out.data_block();
00171 
00172   for (unsigned int i = 0; i < n; ++i) {
00173     double accum = 0;
00174     for (unsigned int k = 0; k < l; ++k)
00175       accum += a[i][k] * bb[k];
00176    outdata[i] = accum;
00177   }
00178 }
00179 
00180 //: Compute $A B^\top$.
00181 void vnl_fastops::ABt(vnl_matrix<double>& out, const vnl_matrix<double>& A, const vnl_matrix<double>& B)
00182 {
00183   const unsigned int na = A.columns();
00184   const unsigned int nb = B.columns();
00185 
00186   // Verify matrices compatible
00187   if (na != nb) {
00188     vcl_cerr << "vnl_fastops::ABt: argument sizes do not match: " << na << " != " << nb << '\n';
00189     vcl_abort();
00190   }
00191 
00192   const unsigned int ma = A.rows();
00193   const unsigned int mb = B.rows();
00194 
00195   // Verify output is the right size
00196   if (out.rows() != ma || out.columns() != mb)
00197     out.set_size(ma,mb);
00198 
00199   double const* const* a = A.data_array();
00200   double const* const* b = B.data_array();
00201   double** outdata = out.data_array();
00202 
00203   for (unsigned int i = 0; i < ma; ++i)
00204     for (unsigned int j = 0; j < mb; ++j) {
00205       double accum = 0;
00206       for (unsigned int k = 0; k < na; ++k)
00207         accum += a[i][k] * b[j][k];
00208       outdata[i][j] = accum;
00209     }
00210 }
00211 
00212 //: Compute $A B A^\top$.
00213 void vnl_fastops::ABAt(vnl_matrix<double>& out, const vnl_matrix<double>& A, const vnl_matrix<double>& B)
00214 {
00215   const unsigned int na = A.columns();
00216   const unsigned int mb = B.rows();
00217 
00218   // Verify matrices compatible
00219   if (na != mb) {
00220     vcl_cerr << "vnl_fastops::ABAt: argument sizes do not match: " << na << " != " << mb << '\n';
00221     vcl_abort();
00222   }
00223 
00224   const unsigned int ma = A.rows();
00225   const unsigned int nb = B.columns();
00226 
00227   // Verify matrices compatible
00228   if (na != nb) {
00229     vcl_cerr << "vnl_fastops::ABAt: argument sizes do not match: " << na << " != " << nb << '\n';
00230     vcl_abort();
00231   }
00232 
00233   // Verify output is the right size
00234   if (out.rows() != ma || out.columns() != ma)
00235     out.set_size(ma,mb);
00236 
00237 
00238   double const* const* a = A.data_array();
00239   double const* const* b = B.data_array();
00240   double** outdata = out.data_array();
00241 
00242   // initialize
00243   for (unsigned int i = 0; i < ma; ++i)
00244     for (unsigned int w = 0; w < ma; ++w)
00245       outdata[i][w] = 0.0;
00246 
00247   for (unsigned int i = 0; i < ma; ++i)
00248     for (unsigned int j = 0; j < nb; ++j) {
00249       double accum = 0;
00250       for (unsigned int k = 0; k < na; ++k)
00251         accum += a[i][k] * b[k][j];
00252       for (unsigned int w = 0; w < ma; ++w)
00253         outdata[i][w] += accum * a[w][j];
00254     }
00255 }
00256 
00257 //: Compute $b^\top A b$ for vector b and matrix A
00258 double vnl_fastops::btAb(const vnl_matrix<double>& A, const vnl_vector<double>& b)
00259 {
00260   const unsigned int m = A.rows();
00261   const unsigned int n = A.cols();
00262   const unsigned int l = b.size();
00263 
00264   // Verify matrices compatible
00265   if (m != l ) {
00266     vcl_cerr << "vnl_fastops::btAb: argument sizes do not match: " << m << " != " << l << '\n';
00267     vcl_abort();
00268   }
00269   if ( m != n ) {
00270     vcl_cerr << "vnl_fastops::btAb: not a square matrix: " << m << " != " << n << '\n';
00271     vcl_abort();
00272   }
00273 
00274 
00275   double const* const* a = A.data_array();
00276   double const* bb = b.data_block();
00277 
00278   double accum = 0;
00279   for (unsigned int i = 0; i < n; ++i)
00280     for (unsigned int j = 0; j < n; ++j) {
00281       accum += bb[j] * a[i][j] * bb[i];
00282     }
00283   return accum;
00284 }
00285 
00286 //: Compute $ X += A^\top A$
00287 void vnl_fastops::inc_X_by_AtA(vnl_matrix<double>& X, const vnl_matrix<double>& A)
00288 {
00289   const unsigned int m = X.rows();
00290   const unsigned int n = X.columns();
00291 
00292   // Verify output is the right size
00293   if (m != n || m != A.columns()) {
00294     vcl_cerr << "vnl_fastops::inc_X_by_AtA: argument sizes do not match\n";
00295     vcl_abort();
00296   }
00297 
00298   const unsigned int l = A.rows();
00299 
00300   double const* const* a = A.data_array();
00301   double** x = X.data_array();
00302 
00303   if (l == 2) {
00304     for (unsigned int i = 0; i < n; ++i) {
00305       x[i][i] += (a[0][i] * a[0][i] + a[1][i] * a[1][i]);
00306       for (unsigned int j = i+1; j < n; ++j) {
00307         double accum = (a[0][i] * a[0][j] + a[1][i] * a[1][j]);
00308         x[i][j] += accum;
00309         x[j][i] += accum;
00310       }
00311     }
00312   } else {
00313     for (unsigned int i = 0; i < n; ++i)
00314       for (unsigned int j = i; j < n; ++j) {
00315         double accum = 0;
00316         for (unsigned int k = 0; k < l; ++k)
00317           accum += a[k][i] * a[k][j];
00318         x[i][j] += accum;
00319         if (i != j)
00320           x[j][i] += accum;
00321       }
00322   }
00323 }
00324 
00325 //: Compute $X += A B$
00326 void vnl_fastops::inc_X_by_AB(vnl_matrix<double>& X, const vnl_matrix<double>& A, const vnl_matrix<double>& B)
00327 {
00328   const unsigned int na = A.columns();
00329   const unsigned int mb = B.rows();
00330 
00331   // Verify matrices compatible
00332   if (na != mb) {
00333     vcl_cerr << "vnl_fastops::inc_X_by_AB: argument sizes do not match: " << na << " != " << mb << '\n';
00334     vcl_abort();
00335   }
00336 
00337   const unsigned int ma = A.rows();
00338   const unsigned int nb = B.columns();
00339   const unsigned int mx = X.rows();
00340   const unsigned int nx = X.columns();
00341 
00342   // Verify output is the right size
00343   if (mx != ma || nx != nb) {
00344     vcl_cerr << "vnl_fastops::inc_X_by_AB: argument sizes do not match\n";
00345     vcl_abort();
00346   }
00347 
00348   double const* const* a = A.data_array();
00349   double const* const* b = B.data_array();
00350   double** x = X.data_array();
00351 
00352   for (unsigned int i = 0; i < ma; ++i)
00353     for (unsigned int j = 0; j < nb; ++j)
00354       for (unsigned int k = 0; k < na; ++k)
00355         x[i][j] += a[i][k] * b[k][j];
00356 }
00357 
00358 //: Compute $X -= A B$
00359 void vnl_fastops::dec_X_by_AB(vnl_matrix<double>& X, const vnl_matrix<double>& A, const vnl_matrix<double>& B)
00360 {
00361   const unsigned int na = A.columns();
00362   const unsigned int mb = B.rows();
00363 
00364   // Verify matrices compatible
00365   if (na != mb) {
00366     vcl_cerr << "vnl_fastops::dec_X_by_AB: argument sizes do not match: " << na << " != " << mb << '\n';
00367     vcl_abort();
00368   }
00369 
00370   const unsigned int ma = A.rows();
00371   const unsigned int nb = B.columns();
00372   const unsigned int mx = X.rows();
00373   const unsigned int nx = X.columns();
00374 
00375   // Verify output is the right size
00376   if (mx != ma || nx != nb) {
00377     vcl_cerr << "vnl_fastops::dec_X_by_AB: argument sizes do not match\n";
00378     vcl_abort();
00379   }
00380 
00381   double const* const* a = A.data_array();
00382   double const* const* b = B.data_array();
00383   double** x = X.data_array();
00384 
00385   for (unsigned int i = 0; i < ma; ++i)
00386     for (unsigned int j = 0; j < nb; ++j)
00387       for (unsigned int k = 0; k < na; ++k)
00388         x[i][j] -= a[i][k] * b[k][j];
00389 }
00390 
00391 //: Compute $X += A^\top B$
00392 void vnl_fastops::inc_X_by_AtB(vnl_matrix<double>& X, const vnl_matrix<double>& A, const vnl_matrix<double>& B)
00393 {
00394   const unsigned int ma = A.rows();
00395   const unsigned int mb = B.rows();
00396 
00397   // Verify matrices compatible
00398   if (ma != mb) {
00399     vcl_cerr << "vnl_fastops::inc_X_by_AtB: argument sizes do not match: " << ma << " != " << mb << '\n';
00400     vcl_abort();
00401   }
00402 
00403   const unsigned int na = A.columns();
00404   const unsigned int nb = B.columns();
00405   const unsigned int mx = X.rows();
00406   const unsigned int nx = X.columns();
00407 
00408   // Verify output is the right size
00409   if (mx != na || nx != nb) {
00410     vcl_cerr << "vnl_fastops::inc_X_by_AtB: argument sizes do not match\n";
00411     vcl_abort();
00412   }
00413 
00414   double const* const* a = A.data_array();
00415   double const* const* b = B.data_array();
00416   double** x = X.data_array();
00417 
00418   for (unsigned int i = 0; i < na; ++i)
00419     for (unsigned int j = 0; j < nb; ++j) {
00420       double accum = 0;
00421       for (unsigned int k = 0; k < ma; ++k)
00422         accum += a[k][i] * b[k][j];
00423       x[i][j] += accum;
00424     }
00425 }
00426 
00427 //: Compute $X -= A^\top B$
00428 void vnl_fastops::dec_X_by_AtB(vnl_matrix<double>& X, const vnl_matrix<double>& A, const vnl_matrix<double>& B)
00429 {
00430   const unsigned int ma = A.rows();
00431   const unsigned int mb = B.rows();
00432 
00433   // Verify matrices compatible
00434   if (ma != mb) {
00435     vcl_cerr << "vnl_fastops::dec_X_by_AtB: argument sizes do not match: " << ma << " != " << mb << '\n';
00436     vcl_abort();
00437   }
00438 
00439   const unsigned int na = A.columns();
00440   const unsigned int nb = B.columns();
00441   const unsigned int mx = X.rows();
00442   const unsigned int nx = X.columns();
00443 
00444   // Verify output is the right size
00445   if (mx != na || nx != nb) {
00446     vcl_cerr << "vnl_fastops::dec_X_by_AtB: argument sizes do not match\n";
00447     vcl_abort();
00448   }
00449 
00450   double const* const* a = A.data_array();
00451   double const* const* b = B.data_array();
00452   double** x = X.data_array();
00453 
00454   for (unsigned int i = 0; i < na; ++i)
00455     for (unsigned int j = 0; j < nb; ++j) {
00456       double accum = 0;
00457       for (unsigned int k = 0; k < ma; ++k)
00458         accum += a[k][i] * b[k][j];
00459       x[i][j] -= accum;
00460     }
00461 }
00462 
00463 //: Compute $X += A^\top b$
00464 void vnl_fastops::inc_X_by_AtB(vnl_vector<double>& X, const vnl_matrix<double>& A, const vnl_vector<double>& B)
00465 {
00466   const unsigned int ma = A.rows();
00467   const unsigned int mb = B.size();
00468 
00469   // Verify matrices compatible
00470   if (ma != mb) {
00471     vcl_cerr << "vnl_fastops::inc_X_by_AtB: argument sizes do not match: " << ma << " != " << mb << '\n';
00472     vcl_abort();
00473   }
00474 
00475   const unsigned int mx = X.size();
00476   const unsigned int na = A.columns();
00477 
00478   // Verify output is the right size
00479   if (mx != na) {
00480     vcl_cerr << "vnl_fastops::inc_X_by_AtB: argument sizes do not match\n";
00481     vcl_abort();
00482   }
00483 
00484   double const* const* a = A.data_array();
00485   double const* b = B.data_block();
00486   double* x = X.data_block();
00487 
00488   for (unsigned int i = 0; i < na; ++i) {
00489     double accum = 0;
00490     for (unsigned int k = 0; k < ma; ++k)
00491       accum += a[k][i] * b[k];
00492     x[i] += accum;
00493   }
00494 }
00495 
00496 //: Compute $X -= A^\top b$
00497 void vnl_fastops::dec_X_by_AtB(vnl_vector<double>& X, const vnl_matrix<double>& A, const vnl_vector<double>& B)
00498 {
00499   const unsigned int ma = A.rows();
00500   const unsigned int mb = B.size();
00501 
00502   // Verify matrices compatible
00503   if (ma != mb) {
00504     vcl_cerr << "vnl_fastops::dec_X_by_AtB: argument sizes do not match: " << ma << " != " << mb << '\n';
00505     vcl_abort();
00506   }
00507 
00508   const unsigned int mx = X.size();
00509   const unsigned int na = A.columns();
00510 
00511   // Verify output is the right size
00512   if (mx != na) {
00513     vcl_cerr << "vnl_fastops::dec_X_by_AtB: argument sizes do not match\n";
00514     vcl_abort();
00515   }
00516 
00517   double const* const* a = A.data_array();
00518   double const* b = B.data_block();
00519   double* x = X.data_block();
00520 
00521   for (unsigned int i = 0; i < na; ++i) {
00522     double accum = 0;
00523     for (unsigned int k = 0; k < ma; ++k)
00524       accum += a[k][i] * b[k];
00525     x[i] -= accum;
00526   }
00527 }
00528 
00529 //: Compute $ X -= A^\top A$
00530 void vnl_fastops::dec_X_by_AtA(vnl_matrix<double>& X, const vnl_matrix<double>& A)
00531 {
00532   const unsigned int m = X.rows();
00533   const unsigned int n = X.columns();
00534 
00535   // Verify output is the right size
00536   if (m != n || m != A.columns()) {
00537     vcl_cerr << "vnl_fastops::dec_X_by_AtA: argument sizes do not match\n";
00538     vcl_abort();
00539   }
00540 
00541   const unsigned int l = A.rows();
00542 
00543   double const* const* a = A.data_array();
00544   double** x = X.data_array();
00545 
00546   if (l == 2) {
00547     for (unsigned int i = 0; i < n; ++i) {
00548       x[i][i] -= (a[0][i] * a[0][i] + a[1][i] * a[1][i]);
00549       for (unsigned int j = i+1; j < n; ++j) {
00550         double accum = (a[0][i] * a[0][j] + a[1][i] * a[1][j]);
00551         x[i][j] -= accum;
00552         x[j][i] -= accum;
00553       }
00554     }
00555   } else {
00556     for (unsigned int i = 0; i < n; ++i)
00557       for (unsigned int j = i; j < n; ++j) {
00558         double accum = 0;
00559         for (unsigned int k = 0; k < l; ++k)
00560           accum += a[k][i] * a[k][j];
00561         x[i][j] -= accum;
00562         if (i != j)
00563           x[j][i] -= accum;
00564       }
00565   }
00566 }
00567 
00568 //: Compute dot product of a and b
00569 double vnl_fastops::dot(const double* a, const double* b, unsigned int n)
00570 {
00571 #define METHOD 3  // Method 2 is fastest on the u170 -- weird.
00572   double accum = 0;
00573 #if METHOD == 1
00574   const double* aend = a + n;
00575   while (a != aend)
00576     accum += *a++ * *b++;
00577 #endif
00578 #if METHOD == 2
00579   for (unsigned int k = 0; k < n; ++k)
00580     accum += a[k] * b[k];
00581 #endif
00582 #if METHOD == 3
00583   while (n--)
00584     accum += a[n] * b[n];
00585 #endif
00586 #if METHOD == 4
00587   unsigned int k = n;
00588   while (k > 0)
00589     --k, accum += a[k] * b[k];
00590 #endif
00591   return accum;
00592 #undef METHOD
00593 }
00594 
00595 //: Compute $X += A B^\top$
00596 void vnl_fastops::inc_X_by_ABt(vnl_matrix<double>& X, const vnl_matrix<double>& A, const vnl_matrix<double>& B)
00597 {
00598   const unsigned int na = A.columns();
00599   const unsigned int nb = B.columns();
00600 
00601   // Verify matrices compatible
00602   if (na != nb) {
00603     vcl_cerr << "vnl_fastops::inc_X_by_ABt: argument sizes do not match: " << na << " != " << nb << '\n';
00604     vcl_abort();
00605   }
00606 
00607   const unsigned int mx = X.rows();
00608   const unsigned int nx = X.columns();
00609   const unsigned int ma = A.rows();
00610   const unsigned int mb = B.rows();
00611 
00612   // Verify output is the right size
00613   if (mx != ma || nx != mb) {
00614     vcl_cerr << "vnl_fastops::inc_X_by_ABt: argument sizes do not match\n";
00615     vcl_abort();
00616   }
00617 
00618   double const* const* a = A.data_array();
00619   double const* const* b = B.data_array();
00620   double** x = X.data_array();
00621 
00622   if (na == 3) {
00623     for (unsigned int i = 0; i < mb; ++i)
00624       for (unsigned int j = 0; j < ma; ++j)
00625         x[j][i] += (a[j][0] * b[i][0] +
00626                     a[j][1] * b[i][1] +
00627                     a[j][2] * b[i][2]);
00628   } else if (na == 2) {
00629     for (unsigned int i = 0; i < mb; ++i)
00630       for (unsigned int j = 0; j < ma; ++j)
00631         x[j][i] += (a[j][0] * b[i][0] +
00632                     a[j][1] * b[i][1]);
00633   } else if (na == 1) {
00634     for (unsigned int i = 0; i < mb; ++i)
00635       for (unsigned int j = 0; j < ma; ++j)
00636         x[j][i] += a[j][0] * b[i][0];
00637   } else {
00638     for (unsigned int i = 0; i < mb; ++i)
00639       for (unsigned int j = 0; j < ma; ++j)
00640         x[j][i] += dot(a[j], b[i], na);
00641   }
00642 }
00643 
00644 //: Compute $X -= A B^\top$
00645 void vnl_fastops::dec_X_by_ABt(vnl_matrix<double>& X, const vnl_matrix<double>& A, const vnl_matrix<double>& B)
00646 {
00647   const unsigned int na = A.columns();
00648   const unsigned int nb = B.columns();
00649 
00650   // Verify matrices compatible
00651   if (na != nb) {
00652     vcl_cerr << "vnl_fastops::dec_X_by_ABt: argument sizes do not match: " << na << " != " << nb << '\n';
00653     vcl_abort();
00654   }
00655 
00656   const unsigned int mx = X.rows();
00657   const unsigned int nx = X.columns();
00658   const unsigned int ma = A.rows();
00659   const unsigned int mb = B.rows();
00660 
00661   // Verify output is the right size
00662   if (mx != ma || nx != mb) {
00663     vcl_cerr << "vnl_fastops::dec_X_by_ABt: argument sizes do not match\n";
00664     vcl_abort();
00665   }
00666 
00667   double const* const* a = A.data_array();
00668   double const* const* b = B.data_array();
00669   double** x = X.data_array();
00670 
00671   if (na == 3) {
00672     for (unsigned int i = 0; i < mb; ++i)
00673       for (unsigned int j = 0; j < ma; ++j)
00674         x[j][i] -= (a[j][0] * b[i][0] +
00675                     a[j][1] * b[i][1] +
00676                     a[j][2] * b[i][2]);
00677   } else if (na == 2) {
00678     for (unsigned int i = 0; i < mb; ++i)
00679       for (unsigned int j = 0; j < ma; ++j)
00680         x[j][i] -= (a[j][0] * b[i][0] +
00681                     a[j][1] * b[i][1]);
00682   } else if (na == 1) {
00683     for (unsigned int i = 0; i < mb; ++i)
00684       for (unsigned int j = 0; j < ma; ++j)
00685         x[j][i] -= a[j][0] * b[i][0];
00686   } else {
00687     for (unsigned int i = 0; i < mb; ++i)
00688       for (unsigned int j = 0; j < ma; ++j)
00689         x[j][i] -= dot(a[j], b[i], na);
00690   }
00691 }
00692 
00693 
00694 //: Compute $X += A B A^\top$.
00695 void vnl_fastops::inc_X_by_ABAt(vnl_matrix<double>& X, const vnl_matrix<double>& A, const vnl_matrix<double>& B)
00696 {
00697   const unsigned int na = A.columns();
00698   const unsigned int mb = B.rows();
00699 
00700   // Verify matrices compatible
00701   if (na != mb) {
00702     vcl_cerr << "vnl_fastops::ABAt: argument sizes do not match: " << na << " != " << mb << '\n';
00703     vcl_abort();
00704   }
00705 
00706   const unsigned int ma = A.rows();
00707   const unsigned int nb = B.columns();
00708 
00709   // Verify matrices compatible
00710   if (na != nb) {
00711     vcl_cerr << "vnl_fastops::ABAt: argument sizes do not match: " << na << " != " << nb << '\n';
00712     vcl_abort();
00713   }
00714 
00715   // Verify output is the right size
00716   if (X.rows() != ma || X.columns() != ma)
00717     X.set_size(ma,mb);
00718 
00719 
00720   double const* const* a = A.data_array();
00721   double const* const* b = B.data_array();
00722   double** Xdata = X.data_array();
00723 
00724   for (unsigned int i = 0; i < ma; ++i)
00725     for (unsigned int j = 0; j < nb; ++j) {
00726       double accum = 0;
00727       for (unsigned int k = 0; k < na; ++k)
00728         accum += a[i][k] * b[k][j];
00729       for (unsigned int w = 0; w < ma; ++w)
00730         Xdata[i][w] += accum * a[w][j];
00731     }
00732 }