core/vnl/vnl_matrix.txx
Go to the documentation of this file.
00001 // This is core/vnl/vnl_matrix.txx
00002 #ifndef vnl_matrix_txx_
00003 #define vnl_matrix_txx_
00004 //:
00005 // \file
00006 //
00007 // Copyright (C) 1991 Texas Instruments Incorporated.
00008 // Copyright (C) 1992 General Electric Company.
00009 //
00010 // Permission is granted to any individual or institution to use, copy, modify,
00011 // and distribute this software, provided that this complete copyright and
00012 // permission notice is maintained, intact, in all copies and supporting
00013 // documentation.
00014 //
00015 // Texas Instruments Incorporated, General Electric Company,
00016 // provides this software "as is" without express or implied warranty.
00017 //
00018 // Created: MBN Apr 21, 1989 Initial design and implementation
00019 // Updated: MBN Jun 22, 1989 Removed non-destructive methods
00020 // Updated: LGO Aug 09, 1989 Inherit from Generic
00021 // Updated: MBN Aug 20, 1989 Changed template usage to reflect new syntax
00022 // Updated: MBN Sep 11, 1989 Added conditional exception handling and base class
00023 // Updated: LGO Oct 05, 1989 Don't re-allocate data in operator= when same size
00024 // Updated: LGO Oct 19, 1989 Add extra parameter to varargs constructor
00025 // Updated: MBN Oct 19, 1989 Added optional argument to set_compare method
00026 // Updated: LGO Dec 08, 1989 Allocate column data in one chunk
00027 // Updated: LGO Dec 08, 1989 Clean-up get and put, add const everywhere.
00028 // Updated: LGO Dec 19, 1989 Remove the map and reduce methods
00029 // Updated: MBN Feb 22, 1990 Changed size arguments from int to unsigned int
00030 // Updated: MJF Jun 30, 1990 Added base class name to constructor initializer
00031 // Updated: VDN Feb 21, 1992 New lite version
00032 // Updated: VDN May 05, 1992 Use envelope to avoid unnecessary copying
00033 // Updated: VDN Sep 30, 1992 Matrix inversion with singular value decomposition
00034 // Updated: AWF Aug 21, 1996 set_identity, normalize_rows, scale_row.
00035 // Updated: AWF Sep 30, 1996 set_row/column methods. Const-correct data_block().
00036 // Updated: AWF 14 Feb 1997  get_n_rows, get_n_columns.
00037 // Updated: PVR 20 Mar 1997  get_row, get_column.
00038 //
00039 // The parameterized vnl_matrix<T> class implements two dimensional arithmetic
00040 // matrices of a user specified type. The only constraint placed on the type is
00041 // that it must overload the following operators: +, -,  *,  and /. Thus, it
00042 // will be possible to have a vnl_matrix over vcl_complex<T>. The vnl_matrix<T>
00043 // class is static in size, that is once a vnl_matrix<T> of a particular size
00044 // has been created, there is no dynamic growth method available. You can
00045 // resize the matrix, with the loss of any existing data using set_size().
00046 //
00047 // Each matrix contains  a protected  data section  that has a T** slot that
00048 // points to the  physical memory allocated  for the two  dimensional array. In
00049 // addition, two integers  specify   the number  of  rows  and columns  for the
00050 // matrix.  These values  are provided in the  constructors. A single protected
00051 // slot  contains a pointer  to a compare  function  to   be used  in  equality
00052 // operations. The default function used is the built-in == operator.
00053 //
00054 // Four  different constructors are provided.  The  first constructor takes two
00055 // integer arguments  specifying the  row  and column  size.   Enough memory is
00056 // allocated to hold row*column elements  of type Type.  The second constructor
00057 // takes the  same two  first arguments, but  also accepts  an additional third
00058 // argument that is  a reference to  an  object of  the appropriate  type whose
00059 // value is used as an initial fill value.  The third constructor is similar to
00060 // the third, except that it accepts a variable number of initialization values
00061 // for the Matrix.  If there are  fewer values than elements,  the rest are set
00062 // to zero. Finally, the last constructor takes a single argument consisting of
00063 // a reference to a Matrix and duplicates its size and element values.
00064 //
00065 // Methods   are  provided   for destructive   scalar   and Matrix    addition,
00066 // multiplication, check for equality  and inequality, fill, reduce, and access
00067 // and set individual elements.  Finally, both  the  input and output operators
00068 // are overloaded to allow for formatted input and output of matrix elements.
00069 //
00070 // Good matrix inversion is needed. We choose singular value decomposition,
00071 // since it is general and works great for nearly singular cases. Singular
00072 // value decomposition is preferred to LU decomposition, since the accuracy
00073 // of the pivots is independent from the left->right top->down elimination.
00074 // LU decomposition also does not give eigenvectors and eigenvalues when
00075 // the matrix is symmetric.
00076 //
00077 // Several different constructors are provided. See .h file for brief descriptions.
00078 
00079 //--------------------------------------------------------------------------------
00080 
00081 #include "vnl_matrix.h"
00082 
00083 #include <vcl_cassert.h>
00084 #include <vcl_cstdio.h>   // EOF
00085 #include <vcl_cstdlib.h>  // abort()
00086 #include <vcl_cctype.h>   // isspace()
00087 #include <vcl_iostream.h>
00088 #include <vcl_vector.h>
00089 #include <vcl_algorithm.h>
00090 
00091 #include <vnl/vnl_math.h>
00092 #include <vnl/vnl_vector.h>
00093 #include <vnl/vnl_c_vector.h>
00094 #include <vnl/vnl_numeric_traits.h>
00095 //--------------------------------------------------------------------------------
00096 
00097 #if VCL_HAS_SLICED_DESTRUCTOR_BUG
00098 // vnl_matrix owns its data by default.
00099 # define vnl_matrix_construct_hack() vnl_matrix_own_data = 1
00100 #else
00101 # define vnl_matrix_construct_hack()
00102 #endif
00103 
00104 // This macro allocates and initializes the dynamic storage used by a vnl_matrix.
00105 #define vnl_matrix_alloc_blah() \
00106 do { \
00107   if (this->num_rows && this->num_cols) { \
00108     /* Allocate memory to hold the row pointers */ \
00109     this->data = vnl_c_vector<T>::allocate_Tptr(this->num_rows); \
00110     /* Allocate memory to hold the elements of the matrix */ \
00111     T* elmns = vnl_c_vector<T>::allocate_T(this->num_rows * this->num_cols); \
00112     /* Fill in the array of row pointers */ \
00113     for (unsigned int i = 0; i < this->num_rows; ++ i) \
00114       this->data[i] = elmns + i*this->num_cols; \
00115   } \
00116   else { \
00117    /* This is to make sure .begin() and .end() work for 0xN matrices: */ \
00118    (this->data = vnl_c_vector<T>::allocate_Tptr(1))[0] = 0; \
00119   } \
00120 } while (false)
00121 
00122 // This macro releases the dynamic storage used by a vnl_matrix.
00123 #define vnl_matrix_free_blah \
00124 do { \
00125   if (this->data) { \
00126     if (this->num_cols && this->num_rows) { \
00127       vnl_c_vector<T>::deallocate(this->data[0], this->num_cols * this->num_rows); \
00128       vnl_c_vector<T>::deallocate(this->data, this->num_rows); \
00129     } \
00130     else { \
00131       vnl_c_vector<T>::deallocate(this->data, 1); \
00132     } \
00133   } \
00134 } while (false)
00135 
00136 //: Creates a matrix with given number of rows and columns.
00137 // Elements are not initialized. O(m*n).
00138 
00139 template <class T>
00140 vnl_matrix<T>::vnl_matrix (unsigned rowz, unsigned colz)
00141 : num_rows(rowz), num_cols(colz)
00142 {
00143   vnl_matrix_construct_hack();
00144   vnl_matrix_alloc_blah();
00145 }
00146 
00147 //: Creates a matrix with given number of rows and columns, and initialize all elements to value. O(m*n).
00148 
00149 template <class T>
00150 vnl_matrix<T>::vnl_matrix (unsigned rowz, unsigned colz, T const& value)
00151 : num_rows(rowz), num_cols(colz)
00152 {
00153   vnl_matrix_construct_hack();
00154   vnl_matrix_alloc_blah();
00155   for (unsigned int i = 0; i < rowz; ++ i)
00156     for (unsigned int j = 0; j < colz; ++ j)
00157       this->data[i][j] = value;
00158 }
00159 
00160 //: r rows, c cols, special type.  Currently implements "identity" and "null".
00161 template <class T>
00162 vnl_matrix<T>::vnl_matrix(unsigned r, unsigned c, vnl_matrix_type t)
00163 : num_rows(r), num_cols(c)
00164 {
00165   vnl_matrix_construct_hack();
00166   vnl_matrix_alloc_blah();
00167   switch (t) {
00168    case vnl_matrix_identity:
00169     assert(r == c);
00170     for (unsigned int i = 0; i < r; ++ i)
00171       for (unsigned int j = 0; j < c; ++ j)
00172         this->data[i][j] = (i==j) ? T(1) : T(0);
00173     break;
00174    case vnl_matrix_null:
00175     for (unsigned int i = 0; i < r; ++ i)
00176       for (unsigned int j = 0; j < c; ++ j)
00177         this->data[i][j] = T(0);
00178     break;
00179    default:
00180     assert(false);
00181     break;
00182   }
00183 }
00184 
00185 #if 1 // fsm: who uses this?
00186 //: Creates a matrix with given dimension (rows, cols) and initialize first n elements, row-wise, to values. O(m*n).
00187 
00188 template <class T>
00189 vnl_matrix<T>::vnl_matrix (unsigned rowz, unsigned colz, unsigned n, T const values[])
00190 : num_rows(rowz), num_cols(colz)
00191 {
00192   vnl_matrix_construct_hack();
00193   vnl_matrix_alloc_blah();
00194   if (n > rowz*colz)
00195     n = rowz*colz;
00196   T *dst = this->data[0];
00197   for (unsigned k=0; k<n; ++k)
00198     dst[k] = values[k];
00199 }
00200 #endif
00201 
00202 //: Creates a matrix from a block array of data, stored row-wise.
00203 // O(m*n).
00204 
00205 template <class T>
00206 vnl_matrix<T>::vnl_matrix (T const* datablck, unsigned rowz, unsigned colz)
00207 : num_rows(rowz), num_cols(colz)
00208 {
00209   vnl_matrix_construct_hack();
00210   vnl_matrix_alloc_blah();
00211   unsigned int n = rowz*colz;
00212   T *dst = this->data[0];
00213   for (unsigned int k=0; k<n; ++k)
00214     dst[k] = datablck[k];
00215 }
00216 
00217 
00218 //: Creates a new matrix and copies all the elements.
00219 // O(m*n).
00220 
00221 template <class T>
00222 vnl_matrix<T>::vnl_matrix (vnl_matrix<T> const& from)
00223 : num_rows(from.num_rows), num_cols(from.num_cols)
00224 {
00225   vnl_matrix_construct_hack();
00226   if (from.data) {
00227     vnl_matrix_alloc_blah();
00228     unsigned int n = this->num_rows * this->num_cols;
00229     T *dst = this->data[0];
00230     T const *src = from.data[0];
00231     for (unsigned int k=0; k<n; ++k)
00232       dst[k] = src[k];
00233   }
00234   else {
00235     num_rows = 0;
00236     num_cols = 0;
00237     data = 0;
00238   }
00239 }
00240 
00241 //------------------------------------------------------------
00242 
00243 template <class T>
00244 vnl_matrix<T>::vnl_matrix (vnl_matrix<T> const &A, vnl_matrix<T> const &B, vnl_tag_add)
00245 : num_rows(A.num_rows), num_cols(A.num_cols)
00246 {
00247 #ifndef NDEBUG
00248   if (A.num_rows != B.num_rows || A.num_cols != B.num_cols)
00249     vnl_error_matrix_dimension ("vnl_tag_add", A.num_rows, A.num_cols, B.num_rows, B.num_cols);
00250 #endif
00251 
00252   vnl_matrix_construct_hack();
00253   vnl_matrix_alloc_blah();
00254 
00255   unsigned int n = A.num_rows * A.num_cols;
00256   T const *a = A.data[0];
00257   T const *b = B.data[0];
00258   T *dst = this->data[0];
00259 
00260   for (unsigned int i=0; i<n; ++i)
00261     dst[i] = T(a[i] + b[i]);
00262 }
00263 
00264 template <class T>
00265 vnl_matrix<T>::vnl_matrix (vnl_matrix<T> const &A, vnl_matrix<T> const &B, vnl_tag_sub)
00266 : num_rows(A.num_rows), num_cols(A.num_cols)
00267 {
00268 #ifndef NDEBUG
00269   if (A.num_rows != B.num_rows || A.num_cols != B.num_cols)
00270     vnl_error_matrix_dimension ("vnl_tag_sub", A.num_rows, A.num_cols, B.num_rows, B.num_cols);
00271 #endif
00272 
00273   vnl_matrix_construct_hack();
00274   vnl_matrix_alloc_blah();
00275 
00276   unsigned int n = A.num_rows * A.num_cols;
00277   T const *a = A.data[0];
00278   T const *b = B.data[0];
00279   T *dst = this->data[0];
00280 
00281   for (unsigned int i=0; i<n; ++i)
00282     dst[i] = T(a[i] - b[i]);
00283 }
00284 
00285 template <class T>
00286 vnl_matrix<T>::vnl_matrix (vnl_matrix<T> const &M, T s, vnl_tag_mul)
00287 : num_rows(M.num_rows), num_cols(M.num_cols)
00288 {
00289   vnl_matrix_construct_hack();
00290   vnl_matrix_alloc_blah();
00291 
00292   unsigned int n = M.num_rows * M.num_cols;
00293   T const *m = M.data[0];
00294   T *dst = this->data[0];
00295 
00296   for (unsigned int i=0; i<n; ++i)
00297     dst[i] = T(m[i] * s);
00298 }
00299 
00300 template <class T>
00301 vnl_matrix<T>::vnl_matrix (vnl_matrix<T> const &M, T s, vnl_tag_div)
00302 : num_rows(M.num_rows), num_cols(M.num_cols)
00303 {
00304   vnl_matrix_construct_hack();
00305   vnl_matrix_alloc_blah();
00306 
00307   unsigned int n = M.num_rows * M.num_cols;
00308   T const *m = M.data[0];
00309   T *dst = this->data[0];
00310 
00311   for (unsigned int i=0; i<n; ++i)
00312     dst[i] = T(m[i] / s);
00313 }
00314 
00315 template <class T>
00316 vnl_matrix<T>::vnl_matrix (vnl_matrix<T> const &M, T s, vnl_tag_add)
00317 : num_rows(M.num_rows), num_cols(M.num_cols)
00318 {
00319   vnl_matrix_construct_hack();
00320   vnl_matrix_alloc_blah();
00321 
00322   unsigned int n = M.num_rows * M.num_cols;
00323   T const *m = M.data[0];
00324   T *dst = this->data[0];
00325 
00326   for (unsigned int i=0; i<n; ++i)
00327     dst[i] = T(m[i] + s);
00328 }
00329 
00330 template <class T>
00331 vnl_matrix<T>::vnl_matrix (vnl_matrix<T> const &M, T s, vnl_tag_sub)
00332 : num_rows(M.num_rows), num_cols(M.num_cols)
00333 {
00334   vnl_matrix_construct_hack();
00335   vnl_matrix_alloc_blah();
00336 
00337   unsigned int n = M.num_rows * M.num_cols;
00338   T const *m = M.data[0];
00339   T *dst = this->data[0];
00340 
00341   for (unsigned int i=0; i<n; ++i)
00342     dst[i] = T(m[i] - s);
00343 }
00344 
00345 template <class T>
00346 vnl_matrix<T>::vnl_matrix (vnl_matrix<T> const &A, vnl_matrix<T> const &B, vnl_tag_mul)
00347 : num_rows(A.num_rows), num_cols(B.num_cols)
00348 {
00349 #ifndef NDEBUG
00350   if (A.num_cols != B.num_rows)
00351     vnl_error_matrix_dimension("vnl_tag_mul", A.num_rows, A.num_cols, B.num_rows, B.num_cols);
00352 #endif
00353 
00354   unsigned int l = A.num_rows;
00355   unsigned int m = A.num_cols; // == B.num_rows
00356   unsigned int n = B.num_cols;
00357 
00358   vnl_matrix_construct_hack();
00359   vnl_matrix_alloc_blah();
00360 
00361   for (unsigned int i=0; i<l; ++i) {
00362     for (unsigned int k=0; k<n; ++k) {
00363       T sum(0);
00364       for (unsigned int j=0; j<m; ++j)
00365         sum += T(A.data[i][j] * B.data[j][k]);
00366       this->data[i][k] = sum;
00367     }
00368   }
00369 }
00370 
00371 //------------------------------------------------------------
00372 
00373 template <class T>
00374 vnl_matrix<T>::~vnl_matrix()
00375 {
00376   // save some fcalls if data is 0 (i.e. in matrix_fixed)
00377 #if VCL_HAS_SLICED_DESTRUCTOR_BUG
00378   if (data && vnl_matrix_own_data) destroy();
00379 #else
00380   if (data) destroy();
00381 #endif
00382 }
00383 
00384 //: Frees up the dynamic storage used by matrix.
00385 // O(m*n).
00386 
00387 template <class T>
00388 void vnl_matrix<T>::destroy()
00389 {
00390   vnl_matrix_free_blah;
00391 }
00392 
00393 template <class T>
00394 void vnl_matrix<T>::clear()
00395 {
00396   if (data) {
00397     destroy();
00398     num_rows = 0;
00399     num_cols = 0;
00400     data = 0;
00401   }
00402 }
00403 
00404 // Resizes the data arrays of THIS matrix to (rows x cols). O(m*n).
00405 // Elements are not initialized, existing data is not preserved.
00406 // Returns true if size is changed.
00407 
00408 template <class T>
00409 bool vnl_matrix<T>::set_size (unsigned rowz, unsigned colz)
00410 {
00411   if (this->data) {
00412     // if no change in size, do not reallocate.
00413     if (this->num_rows == rowz && this->num_cols == colz)
00414       return false;
00415 
00416     // else, simply release old storage and allocate new.
00417     vnl_matrix_free_blah;
00418     this->num_rows = rowz; this->num_cols = colz;
00419     vnl_matrix_alloc_blah();
00420   }
00421   else {
00422     // This happens if the matrix is default constructed.
00423     this->num_rows = rowz; this->num_cols = colz;
00424     vnl_matrix_alloc_blah();
00425   }
00426 
00427   return true;
00428 }
00429 
00430 #undef vnl_matrix_alloc_blah
00431 #undef vnl_matrix_free_blah
00432 
00433 //------------------------------------------------------------
00434 
00435 //: Sets all elements of matrix to specified value. O(m*n).
00436 
00437 template <class T>
00438 vnl_matrix<T>& vnl_matrix<T>::fill (T const& value)
00439 {
00440   for (unsigned int i = 0; i < this->num_rows; i++)
00441     for (unsigned int j = 0; j < this->num_cols; j++)
00442       this->data[i][j] = value;
00443   return *this;
00444 }
00445 
00446 //: Sets all diagonal elements of matrix to specified value. O(n).
00447 
00448 template <class T>
00449 vnl_matrix<T>& vnl_matrix<T>::fill_diagonal (T const& value)
00450 {
00451   for (unsigned int i = 0; i < this->num_rows && i < this->num_cols; ++i)
00452     this->data[i][i] = value;
00453   return *this;
00454 }
00455 
00456 //: Sets the diagonal elements of this matrix to the specified list of values.
00457 
00458 template <class T>
00459 vnl_matrix<T>& vnl_matrix<T>::set_diagonal(vnl_vector<T> const& diag)
00460 {
00461   assert(diag.size() >= this->num_rows ||
00462          diag.size() >= this->num_cols);
00463   // The length of the diagonal of a non-square matrix is the minimum of
00464   // the matrix's width & height; that explains the "||" in the assert,
00465   // and the "&&" in the upper bound for the "for".
00466   for (unsigned int i = 0; i < this->num_rows && i < this->num_cols; ++i)
00467     this->data[i][i] = diag[i];
00468   return *this;
00469 }
00470 
00471 #if 0
00472 //: Assigns value to all elements of a matrix. O(m*n).
00473 
00474 template <class T>
00475 vnl_matrix<T>& vnl_matrix<T>::operator= (T const& value)
00476 {
00477   for (unsigned i = 0; i < this->num_rows; i++)    // For each row in Matrix
00478     for (unsigned j = 0; j < this->num_cols; j++)  // For each column in Matrix
00479       this->data[i][j] = value;                 // Assign value
00480   return *this;                                 // Return Matrix reference
00481 }
00482 #endif // 0
00483 
00484 //: Copies all elements of rhs matrix into lhs matrix. O(m*n).
00485 // If needed, the arrays in lhs matrix are freed up, and new arrays are
00486 // allocated to match the dimensions of the rhs matrix.
00487 
00488 template <class T>
00489 vnl_matrix<T>& vnl_matrix<T>::operator= (vnl_matrix<T> const& rhs)
00490 {
00491   if (this != &rhs) { // make sure *this != m
00492     if (rhs.data) {
00493       this->set_size(rhs.num_rows, rhs.num_cols);
00494       for (unsigned int i = 0; i < this->num_rows; i++)
00495         for (unsigned int j = 0; j < this->num_cols; j++)
00496           this->data[i][j] = rhs.data[i][j];
00497     }
00498     else {
00499       // rhs is default-constructed.
00500       clear();
00501     }
00502   }
00503   return *this;
00504 }
00505 
00506 template <class T>
00507 void vnl_matrix<T>::print(vcl_ostream& os) const
00508 {
00509   for (unsigned int i = 0; i < this->rows(); i++) {
00510     for (unsigned int j = 0; j < this->columns(); j++)
00511       os << this->data[i][j] << ' ';
00512     os << '\n';
00513   }
00514 }
00515 
00516 //: Prints the 2D array of elements of a matrix out to a stream.
00517 // O(m*n).
00518 
00519 template <class T>
00520 vcl_ostream& operator<< (vcl_ostream& os, vnl_matrix<T> const& m)
00521 {
00522   for (unsigned int i = 0; i < m.rows(); ++i) {
00523     for (unsigned int j = 0; j < m.columns(); ++j)
00524       os << m(i, j) << ' ';
00525     os << '\n';
00526   }
00527   return os;
00528 }
00529 
00530 //: Read a vnl_matrix from an ascii vcl_istream.
00531 // Automatically determines file size if the input matrix has zero size.
00532 template <class T>
00533 vcl_istream& operator>>(vcl_istream& s, vnl_matrix<T>& M)
00534 {
00535   M.read_ascii(s);
00536   return s;
00537 }
00538 
00539 template <class T>
00540 void vnl_matrix<T>::inline_function_tickler()
00541 {
00542   vnl_matrix<T> M;
00543   // fsm: hack to get 2.96 to instantiate the inline function.
00544   M = T(1) + T(3) * M;
00545 }
00546 
00547 template <class T>
00548 vnl_matrix<T>& vnl_matrix<T>::operator+= (T value)
00549 {
00550   for (unsigned int i = 0; i < this->num_rows; i++)
00551     for (unsigned int j = 0; j < this->num_cols; j++)
00552       this->data[i][j] += value;
00553   return *this;
00554 }
00555 
00556 template <class T>
00557 vnl_matrix<T>& vnl_matrix<T>::operator-= (T value)
00558 {
00559   for (unsigned int i = 0; i < this->num_rows; i++)
00560     for (unsigned int j = 0; j < this->num_cols; j++)
00561       this->data[i][j] -= value;
00562   return *this;
00563 }
00564 
00565 template <class T>
00566 vnl_matrix<T>& vnl_matrix<T>::operator*= (T value)
00567 {
00568   for (unsigned int i = 0; i < this->num_rows; i++)
00569     for (unsigned int j = 0; j < this->num_cols; j++)
00570       this->data[i][j] *= value;
00571   return *this;
00572 }
00573 
00574 template <class T>
00575 vnl_matrix<T>& vnl_matrix<T>::operator/= (T value)
00576 {
00577   for (unsigned int i = 0; i < this->num_rows; i++)
00578     for (unsigned int j = 0; j < this->num_cols; j++)
00579       this->data[i][j] /= value;
00580   return *this;
00581 }
00582 
00583 
00584 //: Adds lhs matrix with rhs matrix, and stores in place in lhs matrix.
00585 // O(m*n). The dimensions of the two matrices must be identical.
00586 
00587 template <class T>
00588 vnl_matrix<T>& vnl_matrix<T>::operator+= (vnl_matrix<T> const& rhs)
00589 {
00590 #ifndef NDEBUG
00591   if (this->num_rows != rhs.num_rows ||
00592       this->num_cols != rhs.num_cols)           // Size match?
00593     vnl_error_matrix_dimension ("operator+=",
00594                                 this->num_rows, this->num_cols,
00595                                 rhs.num_rows, rhs.num_cols);
00596 #endif
00597   for (unsigned int i = 0; i < this->num_rows; i++)    // For each row
00598     for (unsigned int j = 0; j < this->num_cols; j++)  // For each element in column
00599       this->data[i][j] += rhs.data[i][j];       // Add elements
00600   return *this;
00601 }
00602 
00603 
00604 //: Subtract lhs matrix with rhs matrix and store in place in lhs matrix.
00605 // O(m*n).
00606 // The dimensions of the two matrices must be identical.
00607 
00608 template <class T>
00609 vnl_matrix<T>& vnl_matrix<T>::operator-= (vnl_matrix<T> const& rhs)
00610 {
00611 #ifndef NDEBUG
00612   if (this->num_rows != rhs.num_rows ||
00613       this->num_cols != rhs.num_cols) // Size?
00614     vnl_error_matrix_dimension ("operator-=",
00615                                 this->num_rows, this->num_cols,
00616                                 rhs.num_rows, rhs.num_cols);
00617 #endif
00618   for (unsigned int i = 0; i < this->num_rows; i++)
00619     for (unsigned int j = 0; j < this->num_cols; j++)
00620       this->data[i][j] -= rhs.data[i][j];
00621   return *this;
00622 }
00623 
00624 
00625 template <class T>
00626 vnl_matrix<T> operator- (T const& value, vnl_matrix<T> const& m)
00627 {
00628   vnl_matrix<T> result(m.rows(),m.columns());
00629   for (unsigned int i = 0; i < m.rows(); i++)  // For each row
00630     for (unsigned int j = 0; j < m.columns(); j++) // For each element in column
00631       result.put(i,j, T(value - m.get(i,j)) );    // subtract from value element.
00632   return result;
00633 }
00634 
00635 
00636 #if 0 // commented out
00637 //: Returns new matrix which is the product of m1 with m2, m1 * m2.
00638 // O(n^3). Number of columns of first matrix must match number of rows
00639 // of second matrix.
00640 
00641 template <class T>
00642 vnl_matrix<T> vnl_matrix<T>::operator* (vnl_matrix<T> const& rhs) const
00643 {
00644 #ifndef NDEBUG
00645   if (this->num_cols != rhs.num_rows)           // dimensions do not match?
00646     vnl_error_matrix_dimension("operator*",
00647                                this->num_rows, this->num_cols,
00648                                rhs.num_rows, rhs.num_cols);
00649 #endif
00650   vnl_matrix<T> result(this->num_rows, rhs.num_cols); // Temp to store product
00651   for (unsigned i = 0; i < this->num_rows; i++) {  // For each row
00652     for (unsigned j = 0; j < rhs.num_cols; j++) {  // For each element in column
00653       T sum = 0;
00654       for (unsigned k = 0; k < this->num_cols; k++) // Loop over column values
00655         sum += (this->data[i][k] * rhs.data[k][j]);     // Multiply
00656       result(i,j) = sum;
00657     }
00658   }
00659   return result;
00660 }
00661 #endif
00662 
00663 //: Returns new matrix which is the negation of THIS matrix.
00664 // O(m*n).
00665 
00666 template <class T>
00667 vnl_matrix<T> vnl_matrix<T>::operator- () const
00668 {
00669   vnl_matrix<T> result(this->num_rows, this->num_cols);
00670   for (unsigned int i = 0; i < this->num_rows; i++)
00671     for (unsigned int j = 0; j < this->num_cols; j++)
00672       result.data[i][j] = - this->data[i][j];
00673   return result;
00674 }
00675 
00676 #if 0 // commented out
00677 //: Returns new matrix with elements of lhs matrix added with value.
00678 // O(m*n).
00679 
00680 template <class T>
00681 vnl_matrix<T> vnl_matrix<T>::operator+ (T const& value) const
00682 {
00683   vnl_matrix<T> result(this->num_rows, this->num_cols);
00684   for (unsigned i = 0; i < this->num_rows; i++)    // For each row
00685     for (unsigned j = 0; j < this->num_cols; j++)  // For each element in column
00686       result.data[i][j] = (this->data[i][j] + value);   // Add scalar
00687   return result;
00688 }
00689 
00690 
00691 //: Returns new matrix with elements of lhs matrix multiplied with value.
00692 // O(m*n).
00693 
00694 template <class T>
00695 vnl_matrix<T> vnl_matrix<T>::operator* (T const& value) const
00696 {
00697   vnl_matrix<T> result(this->num_rows, this->num_cols);
00698   for (unsigned i = 0; i < this->num_rows; i++)    // For each row
00699     for (unsigned j = 0; j < this->num_cols; j++)  // For each element in column
00700       result.data[i][j] = (this->data[i][j] * value);   // Multiply
00701   return result;
00702 }
00703 
00704 
00705 //: Returns new matrix with elements of lhs matrix divided by value. O(m*n).
00706 template <class T>
00707 vnl_matrix<T> vnl_matrix<T>::operator/ (T const& value) const
00708 {
00709   vnl_matrix<T> result(this->num_rows, this->num_cols);
00710   for (unsigned i = 0; i < this->num_rows; i++)    // For each row
00711     for (unsigned j = 0; j < this->num_cols; j++)  // For each element in column
00712       result.data[i][j] = (this->data[i][j] / value);   // Divide
00713   return result;
00714 }
00715 #endif
00716 
00717 //: Return the matrix made by applying "f" to each element.
00718 template <class T>
00719 vnl_matrix<T> vnl_matrix<T>::apply(T (*f)(T const&)) const
00720 {
00721   vnl_matrix<T> ret(num_rows, num_cols);
00722   vnl_c_vector<T>::apply(this->data[0], num_rows * num_cols, f, ret.data_block());
00723   return ret;
00724 }
00725 
00726 //: Return the matrix made by applying "f" to each element.
00727 template <class T>
00728 vnl_matrix<T> vnl_matrix<T>::apply(T (*f)(T)) const
00729 {
00730   vnl_matrix<T> ret(num_rows, num_cols);
00731   vnl_c_vector<T>::apply(this->data[0], num_rows * num_cols, f, ret.data_block());
00732   return ret;
00733 }
00734 
00735 ////--------------------------- Additions------------------------------------
00736 
00737 //: Returns new matrix with rows and columns transposed.
00738 // O(m*n).
00739 
00740 template <class T>
00741 vnl_matrix<T> vnl_matrix<T>::transpose() const
00742 {
00743   vnl_matrix<T> result(this->num_cols, this->num_rows);
00744   for (unsigned int i = 0; i < this->num_cols; i++)
00745     for (unsigned int j = 0; j < this->num_rows; j++)
00746       result.data[i][j] = this->data[j][i];
00747   return result;
00748 }
00749 
00750 // adjoint/hermitian transpose
00751 
00752 template <class T>
00753 vnl_matrix<T> vnl_matrix<T>::conjugate_transpose() const
00754 {
00755   vnl_matrix<T> result(transpose());
00756   vnl_c_vector<T>::conjugate(result.begin(),  // src
00757                              result.begin(),  // dst
00758                              result.size());  // size of block
00759   return result;
00760 }
00761 
00762 //: Replaces the submatrix of THIS matrix, starting at top left corner, by the elements of matrix m. O(m*n).
00763 // This is the reverse of extract().
00764 
00765 template <class T>
00766 vnl_matrix<T>& vnl_matrix<T>::update (vnl_matrix<T> const& m,
00767                                       unsigned top, unsigned left)
00768 {
00769   unsigned int bottom = top + m.num_rows;
00770   unsigned int right = left + m.num_cols;
00771 #ifndef NDEBUG
00772   if (this->num_rows < bottom || this->num_cols < right)
00773     vnl_error_matrix_dimension ("update",
00774                                 bottom, right, m.num_rows, m.num_cols);
00775 #endif
00776   for (unsigned int i = top; i < bottom; i++)
00777     for (unsigned int j = left; j < right; j++)
00778       this->data[i][j] = m.data[i-top][j-left];
00779   return *this;
00780 }
00781 
00782 
00783 //: Returns a copy of submatrix of THIS matrix, specified by the top-left corner and size in rows, cols. O(m*n).
00784 // Use update() to copy new values of this submatrix back into THIS matrix.
00785 
00786 template <class T>
00787 vnl_matrix<T> vnl_matrix<T>::extract (unsigned rowz, unsigned colz,
00788                                       unsigned top, unsigned left) const {
00789   vnl_matrix<T> result(rowz, colz);
00790   this->extract( result, top, left );
00791   return result;
00792 }
00793 
00794 template <class T>
00795 void vnl_matrix<T>::extract( vnl_matrix<T>& submatrix,
00796                              unsigned top, unsigned left) const {
00797   unsigned const rowz = submatrix.rows();
00798   unsigned const colz = submatrix.cols();
00799 #ifndef NDEBUG
00800   unsigned int bottom = top + rowz;
00801   unsigned int right = left + colz;
00802   if ((this->num_rows < bottom) || (this->num_cols < right))
00803     vnl_error_matrix_dimension ("extract",
00804                                 this->num_rows, this->num_cols, bottom, right);
00805 #endif
00806   for (unsigned int i = 0; i < rowz; i++)      // actual copy of all elements
00807     for (unsigned int j = 0; j < colz; j++)    // in submatrix
00808       submatrix.data[i][j] = data[top+i][left+j];
00809 }
00810 
00811 //: Returns the dot product of the two matrices. O(m*n).
00812 // This is the sum of all pairwise products of the elements m1[i,j]*m2[i,j].
00813 
00814 template <class T>
00815 T dot_product (vnl_matrix<T> const& m1, vnl_matrix<T> const& m2)
00816 {
00817 #ifndef NDEBUG
00818   if (m1.rows() != m2.rows() || m1.columns() != m2.columns()) // Size?
00819     vnl_error_matrix_dimension ("dot_product",
00820                                 m1.rows(), m1.columns(),
00821                                 m2.rows(), m2.columns());
00822 #endif
00823   return vnl_c_vector<T>::dot_product(m1.begin(), m2.begin(), m1.rows()*m1.cols());
00824 }
00825 
00826 //: Hermitian inner product.
00827 // O(mn).
00828 
00829 template <class T>
00830 T inner_product (vnl_matrix<T> const& m1, vnl_matrix<T> const& m2)
00831 {
00832 #ifndef NDEBUG
00833   if (m1.rows() != m2.rows() || m1.columns() != m2.columns()) // Size?
00834     vnl_error_matrix_dimension ("inner_product",
00835                                 m1.rows(), m1.columns(),
00836                                 m2.rows(), m2.columns());
00837 #endif
00838   return vnl_c_vector<T>::inner_product(m1.begin(), m2.begin(), m1.rows()*m1.cols());
00839 }
00840 
00841 // cos_angle. O(mn).
00842 
00843 template <class T>
00844 T cos_angle (vnl_matrix<T> const& a, vnl_matrix<T> const& b)
00845 {
00846   typedef typename vnl_numeric_traits<T>::abs_t Abs_t;
00847   typedef typename vnl_numeric_traits<Abs_t>::real_t abs_r;
00848 
00849   T ab = inner_product(a,b);
00850   Abs_t a_b = (Abs_t)vcl_sqrt( (abs_r)vnl_math_abs(inner_product(a,a) * inner_product(b,b)) );
00851 
00852   return T( ab / a_b);
00853 }
00854 
00855 //: Returns new matrix whose elements are the products m1[ij]*m2[ij].
00856 // O(m*n).
00857 
00858 template <class T>
00859 vnl_matrix<T> element_product (vnl_matrix<T> const& m1,
00860                                vnl_matrix<T> const& m2)
00861 {
00862 #ifndef NDEBUG
00863   if (m1.rows() != m2.rows() || m1.columns() != m2.columns()) // Size?
00864     vnl_error_matrix_dimension ("element_product",
00865                                 m1.rows(), m1.columns(), m2.rows(), m2.columns());
00866 #endif
00867   vnl_matrix<T> result(m1.rows(), m1.columns());
00868   for (unsigned int i = 0; i < m1.rows(); i++)
00869     for (unsigned int j = 0; j < m1.columns(); j++)
00870       result.put(i,j, T(m1.get(i,j) * m2.get(i,j)) );
00871   return result;
00872 }
00873 
00874 //: Returns new matrix whose elements are the quotients m1[ij]/m2[ij].
00875 // O(m*n).
00876 
00877 template <class T>
00878 vnl_matrix<T> element_quotient (vnl_matrix<T> const& m1,
00879                                 vnl_matrix<T> const& m2)
00880 {
00881 #ifndef NDEBUG
00882   if (m1.rows() != m2.rows() || m1.columns() != m2.columns()) // Size?
00883     vnl_error_matrix_dimension("element_quotient",
00884                                m1.rows(), m1.columns(), m2.rows(), m2.columns());
00885 #endif
00886   vnl_matrix<T> result(m1.rows(), m1.columns());
00887   for (unsigned int i = 0; i < m1.rows(); i++)
00888     for (unsigned int j = 0; j < m1.columns(); j++)
00889       result.put(i,j, T(m1.get(i,j) / m2.get(i,j)) );
00890   return result;
00891 }
00892 
00893 //: Fill this matrix with the given data.
00894 //  We assume that p points to a contiguous rows*cols array, stored rowwise.
00895 template <class T>
00896 vnl_matrix<T>& vnl_matrix<T>::copy_in(T const *p)
00897 {
00898   T* dp = this->data[0];
00899   unsigned int n = this->num_rows * this->num_cols;
00900   while (n--)
00901     *dp++ = *p++;
00902   return *this;
00903 }
00904 
00905 //: Fill the given array with this matrix.
00906 //  We assume that p points to a contiguous rows*cols array, stored rowwise.
00907 template <class T>
00908 void vnl_matrix<T>::copy_out(T *p) const
00909 {
00910   T* dp = this->data[0];
00911   unsigned int n = this->num_rows * this->num_cols;
00912   while (n--)
00913     *p++ = *dp++;
00914 }
00915 
00916 //: Fill this matrix with a matrix having 1s on the main diagonal and 0s elsewhere.
00917 template <class T>
00918 vnl_matrix<T>& vnl_matrix<T>::set_identity()
00919 {
00920   for (unsigned int i = 0; i < this->num_rows; ++i)    // For each row in the Matrix
00921     for (unsigned int j = 0; j < this->num_cols; ++j)  // For each element in column
00922       this->data[i][j] = (i==j) ? T(1) : T(0);
00923   return *this;
00924 }
00925 
00926 //: Make each row of the matrix have unit norm.
00927 // All-zero rows are ignored.
00928 template <class T>
00929 vnl_matrix<T>& vnl_matrix<T>::normalize_rows()
00930 {
00931   typedef typename vnl_numeric_traits<T>::abs_t Abs_t;
00932   typedef typename vnl_numeric_traits<T>::real_t Real_t;
00933   typedef typename vnl_numeric_traits<Real_t>::abs_t abs_real_t;
00934   for (unsigned int i = 0; i < this->num_rows; ++i) {  // For each row in the Matrix
00935     Abs_t norm(0); // double will not do for all types.
00936     for (unsigned int j = 0; j < this->num_cols; ++j)  // For each element in row
00937       norm += vnl_math_squared_magnitude(this->data[i][j]);
00938 
00939     if (norm != 0) {
00940       abs_real_t scale = abs_real_t(1)/(vcl_sqrt((abs_real_t)norm));
00941       for (unsigned int j = 0; j < this->num_cols; ++j)
00942         this->data[i][j] = T(Real_t(this->data[i][j]) * scale);
00943     }
00944   }
00945   return *this;
00946 }
00947 
00948 //: Make each column of the matrix have unit norm.
00949 // All-zero columns are ignored.
00950 template <class T>
00951 vnl_matrix<T>& vnl_matrix<T>::normalize_columns()
00952 {
00953   typedef typename vnl_numeric_traits<T>::abs_t Abs_t;
00954   typedef typename vnl_numeric_traits<T>::real_t Real_t;
00955   typedef typename vnl_numeric_traits<Real_t>::abs_t abs_real_t;
00956   for (unsigned int j = 0; j < this->num_cols; j++) {  // For each column in the Matrix
00957     Abs_t norm(0); // double will not do for all types.
00958     for (unsigned int i = 0; i < this->num_rows; i++)
00959       norm += vnl_math_squared_magnitude(this->data[i][j]);
00960 
00961     if (norm != 0) {
00962       abs_real_t scale = abs_real_t(1)/(vcl_sqrt((abs_real_t)norm));
00963       for (unsigned int i = 0; i < this->num_rows; i++)
00964         this->data[i][j] = T(Real_t(this->data[i][j]) * scale);
00965     }
00966   }
00967   return *this;
00968 }
00969 
00970 //: Multiply row[row_index] by value
00971 template <class T>
00972 vnl_matrix<T>& vnl_matrix<T>::scale_row(unsigned row_index, T value)
00973 {
00974 #ifndef NDEBUG
00975   if (row_index >= this->num_rows)
00976     vnl_error_matrix_row_index("scale_row", row_index);
00977 #endif
00978   for (unsigned int j = 0; j < this->num_cols; j++)    // For each element in row
00979     this->data[row_index][j] *= value;
00980   return *this;
00981 }
00982 
00983 //: Multiply column[column_index] by value
00984 template <class T>
00985 vnl_matrix<T>& vnl_matrix<T>::scale_column(unsigned column_index, T value)
00986 {
00987 #ifndef NDEBUG
00988   if (column_index >= this->num_cols)
00989     vnl_error_matrix_col_index("scale_column", column_index);
00990 #endif
00991   for (unsigned int j = 0; j < this->num_rows; j++)    // For each element in column
00992     this->data[j][column_index] *= value;
00993   return *this;
00994 }
00995 
00996 //: Returns a copy of n rows, starting from "row"
00997 template <class T>
00998 vnl_matrix<T> vnl_matrix<T>::get_n_rows (unsigned row, unsigned n) const
00999 {
01000 #ifndef NDEBUG
01001   if (row + n > this->num_rows)
01002     vnl_error_matrix_row_index ("get_n_rows", row);
01003 #endif
01004 
01005   // Extract data rowwise.
01006   return vnl_matrix<T>(data[row], n, this->num_cols);
01007 }
01008 
01009 //: Returns a copy of n columns, starting from "column".
01010 template <class T>
01011 vnl_matrix<T> vnl_matrix<T>::get_n_columns (unsigned column, unsigned n) const
01012 {
01013 #ifndef NDEBUG
01014   if (column + n > this->num_cols)
01015     vnl_error_matrix_col_index ("get_n_columns", column);
01016 #endif
01017 
01018   vnl_matrix<T> result(this->num_rows, n);
01019   for (unsigned int c = 0; c < n; ++c)
01020     for (unsigned int r = 0; r < this->num_rows; ++r)
01021       result(r, c) = data[r][column + c];
01022   return result;
01023 }
01024 
01025 //: Create a vector out of row[row_index].
01026 template <class T>
01027 vnl_vector<T> vnl_matrix<T>::get_row(unsigned row_index) const
01028 {
01029 #ifdef ERROR_CHECKING
01030   if (row_index >= this->num_rows)
01031     vnl_error_matrix_row_index ("get_row", row_index);
01032 #endif
01033 
01034   vnl_vector<T> v(this->num_cols);
01035   for (unsigned int j = 0; j < this->num_cols; j++)    // For each element in row
01036     v[j] = this->data[row_index][j];
01037   return v;
01038 }
01039 
01040 //: Create a vector out of column[column_index].
01041 template <class T>
01042 vnl_vector<T> vnl_matrix<T>::get_column(unsigned column_index) const
01043 {
01044 #ifdef ERROR_CHECKING
01045   if (column_index >= this->num_cols)
01046     vnl_error_matrix_col_index ("get_column", column_index);
01047 #endif
01048 
01049   vnl_vector<T> v(this->num_rows);
01050   for (unsigned int j = 0; j < this->num_rows; j++)    // For each element in row
01051     v[j] = this->data[j][column_index];
01052   return v;
01053 }
01054 
01055 //: Return a vector with the content of the (main) diagonal
01056 template <class T>
01057 vnl_vector<T> vnl_matrix<T>::get_diagonal() const
01058 {
01059   vnl_vector<T> v(this->num_rows < this->num_cols ? this->num_rows : this->num_cols);
01060   for (unsigned int j = 0; j < this->num_rows && j < this->num_cols; ++j)
01061     v[j] = this->data[j][j];
01062   return v;
01063 }
01064 
01065 //--------------------------------------------------------------------------------
01066 
01067 //: Set row[row_index] to data at given address. No bounds check.
01068 template <class T>
01069 vnl_matrix<T>& vnl_matrix<T>::set_row(unsigned row_index, T const *v)
01070 {
01071   for (unsigned int j = 0; j < this->num_cols; j++)    // For each element in row
01072     this->data[row_index][j] = v[j];
01073   return *this;
01074 }
01075 
01076 //: Set row[row_index] to given vector.
01077 template <class T>
01078 vnl_matrix<T>& vnl_matrix<T>::set_row(unsigned row_index, vnl_vector<T> const &v)
01079 {
01080 #ifndef NDEBUG
01081   if (v.size() != this->num_cols)
01082     vnl_error_vector_dimension ("vnl_matrix::set_row", v.size(), this->num_cols);
01083 #endif
01084   set_row(row_index,v.data_block());
01085   return *this;
01086 }
01087 
01088 //: Set row[row_index] to given value.
01089 template <class T>
01090 vnl_matrix<T>& vnl_matrix<T>::set_row(unsigned row_index, T v)
01091 {
01092   for (unsigned int j = 0; j < this->num_cols; j++)    // For each element in row
01093     this->data[row_index][j] = v;
01094   return *this;
01095 }
01096 
01097 //--------------------------------------------------------------------------------
01098 
01099 //: Set column[column_index] to data at given address.
01100 template <class T>
01101 vnl_matrix<T>& vnl_matrix<T>::set_column(unsigned column_index, T const *v)
01102 {
01103   for (unsigned int i = 0; i < this->num_rows; i++)    // For each element in row
01104     this->data[i][column_index] = v[i];
01105   return *this;
01106 }
01107 
01108 //: Set column[column_index] to given vector.
01109 template <class T>
01110 vnl_matrix<T>& vnl_matrix<T>::set_column(unsigned column_index, vnl_vector<T> const &v)
01111 {
01112 #ifndef NDEBUG
01113   if (v.size() != this->num_rows)
01114     vnl_error_vector_dimension ("vnl_matrix::set_column", v.size(), this->num_rows);
01115 #endif
01116   set_column(column_index,v.data_block());
01117   return *this;
01118 }
01119 
01120 //: Set column[column_index] to given value.
01121 template <class T>
01122 vnl_matrix<T>& vnl_matrix<T>::set_column(unsigned column_index, T v)
01123 {
01124   for (unsigned int j = 0; j < this->num_rows; j++)    // For each element in row
01125     this->data[j][column_index] = v;
01126   return *this;
01127 }
01128 
01129 
01130 //: Set columns starting at starting_column to given matrix
01131 template <class T>
01132 vnl_matrix<T>& vnl_matrix<T>::set_columns(unsigned starting_column, vnl_matrix<T> const& m)
01133 {
01134 #ifndef NDEBUG
01135   if (this->num_rows != m.num_rows ||
01136       this->num_cols < m.num_cols + starting_column)           // Size match?
01137     vnl_error_matrix_dimension ("set_columns",
01138                                 this->num_rows, this->num_cols,
01139                                 m.num_rows, m.num_cols);
01140 #endif
01141 
01142   for (unsigned int j = 0; j < m.num_cols; ++j)
01143     for (unsigned int i = 0; i < this->num_rows; i++)    // For each element in row
01144       this->data[i][starting_column + j] = m.data[i][j];
01145   return *this;
01146 }
01147 
01148 //--------------------------------------------------------------------------------
01149 
01150 //: Two matrices are equal if and only if they have the same dimensions and the same values.
01151 // O(m*n).
01152 // Elements are compared with operator== as default.
01153 // Change this default with set_compare() at run time or by specializing
01154 // vnl_matrix_compare at compile time.
01155 
01156 template <class T>
01157 bool vnl_matrix<T>::operator_eq(vnl_matrix<T> const& rhs) const
01158 {
01159   if (this == &rhs)                                      // same object => equal.
01160     return true;
01161 
01162   if (this->num_rows != rhs.num_rows || this->num_cols != rhs.num_cols)
01163     return false;                                        // different sizes => not equal.
01164 
01165   for (unsigned int i = 0; i < this->num_rows; i++)     // For each row
01166     for (unsigned int j = 0; j < this->num_cols; j++)   // For each column
01167       if (!(this->data[i][j] == rhs.data[i][j]))            // different element ?
01168         return false;                                    // Then not equal.
01169 
01170   return true;                                           // Else same; return true
01171 }
01172 
01173 template <class T>
01174 bool vnl_matrix<T>::is_equal(vnl_matrix<T> const& rhs, double tol) const
01175 {
01176   if (this == &rhs)                                      // same object => equal.
01177     return true;
01178 
01179   if (this->num_rows != rhs.num_rows || this->num_cols != rhs.num_cols)
01180     return false;                                        // different sizes => not equal.
01181 
01182   for (unsigned int i = 0; i < this->rows(); ++i)
01183     for (unsigned int j = 0; j < this->columns(); ++j)
01184       if (vnl_math_abs(this->data[i][j] - rhs.data[i][j]) > tol)
01185         return false;                                    // difference greater than tol
01186 
01187   return true;
01188 }
01189 
01190 
01191 template <class T>
01192 bool vnl_matrix<T>::is_identity() const
01193 {
01194   T const zero(0);
01195   T const one(1);
01196   for (unsigned int i = 0; i < this->rows(); ++i)
01197     for (unsigned int j = 0; j < this->columns(); ++j) {
01198       T xm = (*this)(i,j);
01199       if ( !((i == j) ? (xm == one) : (xm == zero)) )
01200         return false;
01201     }
01202   return true;
01203 }
01204 
01205 //: Return true if maximum absolute deviation of M from identity is <= tol.
01206 template <class T>
01207 bool vnl_matrix<T>::is_identity(double tol) const
01208 {
01209   T one(1);
01210   for (unsigned int i = 0; i < this->rows(); ++i)
01211     for (unsigned int j = 0; j < this->columns(); ++j) {
01212       T xm = (*this)(i,j);
01213       abs_t absdev = (i == j) ? vnl_math_abs(xm - one) : vnl_math_abs(xm);
01214       if (absdev > tol)
01215         return false;
01216     }
01217   return true;
01218 }
01219 
01220 template <class T>
01221 bool vnl_matrix<T>::is_zero() const
01222 {
01223   T const zero(0);
01224   for (unsigned int i = 0; i < this->rows(); ++i)
01225     for (unsigned int j = 0; j < this->columns(); ++j)
01226       if ( !( (*this)(i, j) == zero) )
01227         return false;
01228 
01229   return true;
01230 }
01231 
01232 //: Return true if max(abs((*this))) <= tol.
01233 template <class T>
01234 bool vnl_matrix<T>::is_zero(double tol) const
01235 {
01236   for (unsigned int i = 0; i < this->rows(); ++i)
01237     for (unsigned int j = 0; j < this->columns(); ++j)
01238       if (vnl_math_abs((*this)(i,j)) > tol)
01239         return false;
01240 
01241   return true;
01242 }
01243 
01244 //: Return true if any element of (*this) is nan
01245 template <class T>
01246 bool vnl_matrix<T>::has_nans() const
01247 {
01248   for (unsigned int i = 0; i < this->rows(); ++i)
01249     for (unsigned int j = 0; j < this->columns(); ++j)
01250       if (vnl_math_isnan((*this)(i,j)))
01251         return true;
01252 
01253   return false;
01254 }
01255 
01256 //: Return false if any element of (*this) is inf or nan
01257 template <class T>
01258 bool vnl_matrix<T>::is_finite() const
01259 {
01260   for (unsigned int i = 0; i < this->rows(); ++i)
01261     for (unsigned int j = 0; j < this->columns(); ++j)
01262       if (!vnl_math_isfinite((*this)(i,j)))
01263         return false;
01264 
01265   return true;
01266 }
01267 
01268 //: Abort if any element of M is inf or nan
01269 template <class T>
01270 void vnl_matrix<T>::assert_finite_internal() const
01271 {
01272   if (is_finite())
01273     return;
01274 
01275   vcl_cerr << "\n\n" __FILE__ ": " << __LINE__ << ": matrix has non-finite elements\n";
01276 
01277   if (rows() <= 20 && cols() <= 20) {
01278     vcl_cerr << __FILE__ ": here it is:\n" << *this;
01279   }
01280   else {
01281     vcl_cerr << __FILE__ ": it is quite big (" << rows() << 'x' << cols() << ")\n"
01282              << __FILE__ ": in the following picture '-' means finite and '*' means non-finite:\n";
01283 
01284     for (unsigned int i=0; i<rows(); ++i) {
01285       for (unsigned int j=0; j<cols(); ++j)
01286         vcl_cerr << char(vnl_math_isfinite((*this)(i, j)) ? '-' : '*');
01287       vcl_cerr << '\n';
01288     }
01289   }
01290   vcl_cerr << __FILE__ ": calling abort()\n";
01291   vcl_abort();
01292 }
01293 
01294 //: Abort unless M has the given size.
01295 template <class T>
01296 void vnl_matrix<T>::assert_size_internal(unsigned rs,unsigned cs) const
01297 {
01298   if (this->rows()!=rs || this->cols()!=cs) {
01299     vcl_cerr << __FILE__ ": size is " << this->rows() << 'x' << this->cols()
01300              << ". should be " << rs << 'x' << cs << vcl_endl;
01301     vcl_abort();
01302   }
01303 }
01304 
01305 //: Read a vnl_matrix from an ascii vcl_istream.
01306 // Automatically determines file size if the input matrix has zero size.
01307 template <class T>
01308 bool vnl_matrix<T>::read_ascii(vcl_istream& s)
01309 {
01310   if (!s.good()) {
01311     vcl_cerr << __FILE__ ": vnl_matrix<T>::read_ascii: Called with bad stream\n";
01312     return false;
01313   }
01314 
01315   bool size_known = (this->rows() != 0);
01316 
01317   if (size_known) {
01318     for (unsigned int i = 0; i < this->rows(); ++i)
01319       for (unsigned int j = 0; j < this->columns(); ++j)
01320         s >> this->data[i][j];
01321 
01322     return s.good() || s.eof();
01323   }
01324 
01325   bool debug = false;
01326 
01327   vcl_vector<T> first_row_vals;
01328   if (debug)
01329     vcl_cerr << __FILE__ ": vnl_matrix<T>::read_ascii: Determining file dimensions: ";
01330 
01331   for (;;) {
01332     // Clear whitespace, looking for a newline
01333     while (true)
01334     {
01335       int c = s.get();
01336       if (c == EOF)
01337         goto loademup;
01338       if (!vcl_isspace(c)) {
01339         if (!s.putback(char(c)).good())
01340           vcl_cerr << "vnl_matrix<T>::read_ascii: Could not push back '" << c << "'\n";
01341 
01342         goto readfloat;
01343       }
01344       // First newline after first number tells us the column dimension
01345       if (c == '\n' && first_row_vals.size() > 0) {
01346         goto loademup;
01347       }
01348     }
01349   readfloat:
01350     T val;
01351     s >> val;
01352     if (!s.fail())
01353       first_row_vals.push_back(val);
01354     if (s.eof())
01355       goto loademup;
01356   }
01357  loademup:
01358   vcl_size_t colz = first_row_vals.size();
01359 
01360   if (debug) vcl_cerr << colz << " cols, ";
01361 
01362   if (colz == 0)
01363     return false;
01364 
01365   // need to be careful with resizing here as will often be reading humungous files
01366   // So let's just build an array of row pointers
01367   vcl_vector<T*> row_vals;
01368   row_vals.reserve(1000);
01369   {
01370     // Copy first row.  Can't use first_row_vals, as may be a vector of bool...
01371     T* row = vnl_c_vector<T>::allocate_T(colz);
01372     for (unsigned int k = 0; k < colz; ++k)
01373       row[k] = first_row_vals[k];
01374     row_vals.push_back(row);
01375   }
01376 
01377   while (true)
01378   {
01379     T* row = vnl_c_vector<T>::allocate_T(colz);
01380     if (row == 0) {
01381       vcl_cerr << "vnl_matrix<T>::read_ascii: Error, Out of memory on row "
01382                << row_vals.size() << vcl_endl;
01383       return false;
01384     }
01385     s >> row[0];
01386     if (!s.good())
01387     {
01388       vnl_c_vector<T>::deallocate(row, colz);
01389       break;
01390     }
01391     for (unsigned int k = 1; k < colz; ++k) {
01392       if (s.eof()) {
01393         vcl_cerr << "vnl_matrix<T>::read_ascii: Error, EOF on row "
01394                  << row_vals.size() << ", column " << k << vcl_endl;
01395 
01396         return false;
01397       }
01398       s >> row[k];
01399       if (s.fail()) {
01400         vcl_cerr << "vnl_matrix<T>::read_ascii: Error, row "
01401                  << row_vals.size() << " failed on column " << k << vcl_endl;
01402         return false;
01403       }
01404     }
01405     row_vals.push_back(row);
01406   }
01407 
01408   vcl_size_t rowz = row_vals.size();
01409 
01410   if (debug)
01411     vcl_cerr << rowz << " rows.\n";
01412 
01413   set_size((unsigned int)rowz, (unsigned int)colz);
01414 
01415   T* p = this->data[0];
01416   for (unsigned int i = 0; i < rowz; ++i) {
01417     for (unsigned int j = 0; j < colz; ++j)
01418       *p++ = row_vals[i][j];
01419     /*if (i>0)*/ vnl_c_vector<T>::deallocate(row_vals[i], colz);
01420   }
01421 
01422   return true;
01423 }
01424 
01425 //: Read a vnl_matrix from an ascii vcl_istream.
01426 // Automatically determines file size if the input matrix has zero size.
01427 // This is a static method so you can type
01428 // <verb>
01429 // vnl_matrix<float> M = vnl_matrix<float>::read(cin);
01430 // </verb>
01431 // which many people prefer to the ">>" alternative.
01432 template <class T>
01433 vnl_matrix<T> vnl_matrix<T>::read(vcl_istream& s)
01434 {
01435   vnl_matrix<T> M;
01436   s >> M;
01437   return M;
01438 }
01439 
01440 template <class T>
01441 void vnl_matrix<T>::swap(vnl_matrix<T> &that)
01442 {
01443   vcl_swap(this->num_rows, that.num_rows);
01444   vcl_swap(this->num_cols, that.num_cols);
01445   vcl_swap(this->data, that.data);
01446 }
01447 
01448 //: Reverse order of rows.  Name is from Matlab, meaning "flip upside down".
01449 template <class T>
01450 vnl_matrix<T>& vnl_matrix<T>::flipud()
01451 {
01452   unsigned int n = this->rows();
01453   unsigned int colz = this->columns();
01454 
01455   unsigned int m = n / 2;
01456   for (unsigned int r = 0; r < m; ++r) {
01457     unsigned int r1 = r;
01458     unsigned int r2 = n - 1 - r;
01459     for (unsigned int c = 0; c < colz; ++c) {
01460       T tmp = (*this)(r1, c);
01461       (*this)(r1, c) = (*this)(r2, c);
01462       (*this)(r2, c) = tmp;
01463     }
01464   }
01465   return *this;
01466 }
01467 
01468 //: Reverse order of columns.
01469 template <class T>
01470 vnl_matrix<T>& vnl_matrix<T>::fliplr()
01471 {
01472   unsigned int n = this->cols();
01473   unsigned int rowz = this->rows();
01474 
01475   unsigned int m = n / 2;
01476   for (unsigned int c = 0; c < m; ++c) {
01477     unsigned int c1 = c;
01478     unsigned int c2 = n - 1 - c;
01479     for (unsigned int r = 0; r < rowz; ++r) {
01480       T tmp = (*this)(r, c1);
01481       (*this)(r, c1) = (*this)(r, c2);
01482       (*this)(r, c2) = tmp;
01483     }
01484   }
01485   return *this;
01486 }
01487 
01488 // || M ||  = \max \sum | M   |
01489 //        1     j    i     ij
01490 template <class T>
01491 typename vnl_matrix<T>::abs_t vnl_matrix<T>::operator_one_norm() const
01492 {
01493   abs_t max = 0;
01494   for (unsigned int j=0; j<this->num_cols; ++j) {
01495     abs_t tmp = 0;
01496     for (unsigned int i=0; i<this->num_rows; ++i)
01497       tmp += vnl_math_abs(this->data[i][j]);
01498     if (tmp > max)
01499       max = tmp;
01500   }
01501   return max;
01502 }
01503 
01504 // || M ||   = \max \sum | M   |
01505 //        oo     i    j     ij
01506 template <class T>
01507 typename vnl_matrix<T>::abs_t vnl_matrix<T>::operator_inf_norm() const
01508 {
01509   abs_t max = 0;
01510   for (unsigned int i=0; i<this->num_rows; ++i) {
01511     abs_t tmp = 0;
01512     for (unsigned int j=0; j<this->num_cols; ++j)
01513       tmp += vnl_math_abs(this->data[i][j]);
01514     if (tmp > max)
01515       max = tmp;
01516   }
01517   return max;
01518 }
01519 
01520 template <class doublereal>              // ideally, char* should be bool* - PVr
01521 int vnl_inplace_transpose(doublereal *a, unsigned m, unsigned n, char* move, unsigned iwrk)
01522 {
01523   doublereal b, c;
01524   int k = m * n - 1;
01525   int iter, i1, i2, im, i1c, i2c, ncount, max_;
01526 
01527 // *****
01528 //  ALGORITHM 380 - REVISED
01529 // *****
01530 //  A IS A ONE-DIMENSIONAL ARRAY OF LENGTH MN=M*N, WHICH
01531 //  CONTAINS THE MXN MATRIX TO BE TRANSPOSED (STORED
01532 //  COLUMNWISE). MOVE IS A ONE-DIMENSIONAL ARRAY OF LENGTH IWRK
01533 //  USED TO STORE INFORMATION TO SPEED UP THE PROCESS.  THE
01534 //  VALUE IWRK=(M+N)/2 IS RECOMMENDED. IOK INDICATES THE
01535 //  SUCCESS OR FAILURE OF THE ROUTINE.
01536 //  NORMAL RETURN  IOK=0
01537 //  ERRORS         IOK=-2 ,IWRK NEGATIVE OR ZERO
01538 //                 IOK.GT.0, (SHOULD NEVER OCCUR),IN THIS CASE
01539 //  WE SET IOK EQUAL TO THE FINAL VALUE OF ITER WHEN THE SEARCH
01540 //  IS COMPLETED BUT SOME LOOPS HAVE NOT BEEN MOVED
01541 //  NOTE * MOVE(I) WILL STAY ZERO FOR FIXED POINTS
01542 
01543   if (m < 2 || n < 2)
01544     return 0; // JUST RETURN IF MATRIX IS SINGLE ROW OR COLUMN
01545   if (iwrk < 1)
01546     return -2; // ERROR RETURN
01547   if (m == n) {
01548     // IF MATRIX IS SQUARE, EXCHANGE ELEMENTS A(I,J) AND A(J,I).
01549     for (unsigned i = 0; i < n; ++i)
01550     for (unsigned j = i+1; j < n; ++j) {
01551       i1 = i + j * n;
01552       i2 = j + i * m;
01553       b = a[i1];
01554       a[i1] = a[i2];
01555       a[i2] = b;
01556     }
01557     return 0; // NORMAL RETURN
01558   }
01559   ncount = 2;
01560   for (unsigned i = 0; i < iwrk; ++i)
01561     move[i] = char(0); // false;
01562   if (m > 2 && n > 2) {
01563     // CALCULATE THE NUMBER OF FIXED POINTS, EUCLIDS ALGORITHM FOR GCD(M-1,N-1).
01564     int ir2 = m - 1;
01565     int ir1 = n - 1;
01566     int ir0 = ir2 % ir1;
01567     while (ir0 != 0) {
01568       ir2 = ir1;
01569       ir1 = ir0;
01570       ir0 = ir2 % ir1;
01571     }
01572     ncount += ir1 - 1;
01573   }
01574 // SET INITIAL VALUES FOR SEARCH
01575   iter = 1;
01576   im = m;
01577 // AT LEAST ONE LOOP MUST BE RE-ARRANGED
01578   goto L80;
01579 // SEARCH FOR LOOPS TO REARRANGE
01580 L40:
01581   max_ = k - iter;
01582   ++iter;
01583   if (iter > max_)
01584     return iter; // error return
01585   im += m;
01586   if (im > k)
01587     im -= k;
01588   i2 = im;
01589   if (iter == i2)
01590     goto L40;
01591   if (iter <= (int)iwrk) {
01592     if (move[iter-1])
01593       goto L40;
01594     else
01595       goto L80;
01596   }
01597   while (i2 > iter && i2 < max_) {
01598     i1 = i2;
01599     i2 = m * i1 - k * (i1 / n);
01600   }
01601   if (i2 != iter)
01602     goto L40;
01603 // REARRANGE THE ELEMENTS OF A LOOP AND ITS COMPANION LOOP
01604 L80:
01605   i1 = iter;
01606   b = a[i1];
01607   i1c = k - iter;
01608   c = a[i1c];
01609   while (true) {
01610     i2 = m * i1 - k * (i1 / n);
01611     i2c = k - i2;
01612     if (i1 <= (int)iwrk)
01613       move[i1-1] = '1'; // true;
01614     if (i1c <= (int)iwrk)
01615       move[i1c-1] = '1'; // true;
01616     ncount += 2;
01617     if (i2 == iter)
01618       break;
01619     if (i2+iter == k) {
01620       doublereal d = b; b = c; c = d; // interchange b and c
01621       break;
01622     }
01623     a[i1] = a[i2];
01624     a[i1c] = a[i2c];
01625     i1 = i2;
01626     i1c = i2c;
01627   }
01628 // FINAL STORE AND TEST FOR FINISHED
01629   a[i1] = b;
01630   a[i1c] = c;
01631   if (ncount > k)
01632     return 0; // NORMAL RETURN
01633   goto L40;
01634 } /* dtrans_ */
01635 
01636 
01637 //: Transpose matrix M in place.
01638 //  Works for rectangular matrices using an enormously clever algorithm from ACM TOMS.
01639 template <class T>
01640 vnl_matrix<T>& vnl_matrix<T>::inplace_transpose()
01641 {
01642   unsigned m = rows();
01643   unsigned n = columns();
01644   unsigned iwrk = (m+n)/2;
01645   vcl_vector<char> move(iwrk);
01646 
01647   int iok = ::vnl_inplace_transpose(data_block(), n, m, &move[0], iwrk);
01648   if (iok != 0)
01649     vcl_cerr << __FILE__ " : inplace_transpose() -- iok = " << iok << vcl_endl;
01650 
01651   this->num_rows = n;
01652   this->num_cols = m;
01653 
01654   // row pointers. we have to reallocate even when n<=m because
01655   // vnl_c_vector<T>::deallocate needs to know n_when_allocatod.
01656   {
01657     T *tmp = data[0];
01658     vnl_c_vector<T>::deallocate(data, m);
01659     data = vnl_c_vector<T>::allocate_Tptr(n);
01660     for (unsigned i=0; i<n; ++i)
01661       data[i] = tmp + i * m;
01662   }
01663   return *this;
01664 }
01665 
01666 //------------------------------------------------------------------------------
01667 
01668 #define VNL_MATRIX_INSTANTIATE(T) \
01669 template class vnl_matrix<T >; \
01670 template vnl_matrix<T > operator-(T const &, vnl_matrix<T > const &); \
01671 VCL_INSTANTIATE_INLINE(vnl_matrix<T > operator+(T const &, vnl_matrix<T > const &)); \
01672 VCL_INSTANTIATE_INLINE(vnl_matrix<T > operator*(T const &, vnl_matrix<T > const &)); \
01673 template T dot_product(vnl_matrix<T > const &, vnl_matrix<T > const &); \
01674 template T inner_product(vnl_matrix<T > const &, vnl_matrix<T > const &); \
01675 template T cos_angle(vnl_matrix<T > const &, vnl_matrix<T > const &); \
01676 template vnl_matrix<T > element_product(vnl_matrix<T > const &, vnl_matrix<T > const &); \
01677 template vnl_matrix<T > element_quotient(vnl_matrix<T > const &, vnl_matrix<T > const &); \
01678 template int vnl_inplace_transpose(T*, unsigned, unsigned, char*, unsigned); \
01679 template vcl_ostream & operator<<(vcl_ostream &, vnl_matrix<T > const &); \
01680 template vcl_istream & operator>>(vcl_istream &, vnl_matrix<T >       &)
01681 
01682 #endif // vnl_matrix_txx_