1 //============================================================================== 2 // 3 // This file is part of GPSTk, the GPS Toolkit. 4 // 5 // The GPSTk is free software; you can redistribute it and/or modify 6 // it under the terms of the GNU Lesser General Public License as published 7 // by the Free Software Foundation; either version 3.0 of the License, or 8 // any later version. 9 // 10 // The GPSTk is distributed in the hope that it will be useful, 11 // but WITHOUT ANY WARRANTY; without even the implied warranty of 12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 // GNU Lesser General Public License for more details. 14 // 15 // You should have received a copy of the GNU Lesser General Public 16 // License along with GPSTk; if not, write to the Free Software Foundation, 17 // Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA 18 // 19 // This software was developed by Applied Research Laboratories at the 20 // University of Texas at Austin. 21 // Copyright 2004-2020, The Board of Regents of The University of Texas System 22 // 23 //============================================================================== 24 25 //============================================================================== 26 // 27 // This software was developed by Applied Research Laboratories at the 28 // University of Texas at Austin, under contract to an agency or agencies 29 // within the U.S. Department of Defense. The U.S. Government retains all 30 // rights to use, duplicate, distribute, disclose, or release this software. 31 // 32 // Pursuant to DoD Directive 523024 33 // 34 // DISTRIBUTION STATEMENT A: This software has been approved for public 35 // release, distribution is unlimited. 36 // 37 //============================================================================== 38 /// @file SparseMatrix.hpp Class for template sparse matrices; use with SparseVector. 39 40 #ifndef SPARSE_MATRIX_INCLUDE 41 #define SPARSE_MATRIX_INCLUDE 42 43 #include <map> 44 #include <string> 45 #include <algorithm> // for find,lower_bound 46 47 #include "SparseVector.hpp" 48 #include "Matrix.hpp" 49 50 namespace gpstk 51 { 52 // forward declarations 53 template <class T> class SparseMatrix; 54 55 //--------------------------------------------------------------------------- 56 /// Proxy class for elements of the SparseMatrix (SM). 57 template <class T> class SMatProxy 58 { 59 public: 60 /// constructor 61 SMatProxy(SparseMatrix<T>& SM, unsigned int i, unsigned int j); 62 63 /// operator = for non-const (lvalue) operator =(const SMatProxy<T> & rhs)64 SMatProxy& operator=(const SMatProxy<T>& rhs) 65 { assign(rhs); return *this; } 66 /// operator = for const (rvalue) operator =(T rhs)67 SMatProxy& operator=(T rhs) 68 { assign(rhs); return *this; } 69 70 /// cast or implicit conversion 71 operator T() const; 72 73 /// operator+= for non-const (lvalue) operator +=(const SMatProxy<T> & rhs)74 SMatProxy& operator+=(const SMatProxy<T>& rhs) 75 { assign(value()+rhs); return *this; } 76 77 /// operator+= for const (rvalue) operator +=(T rhs)78 SMatProxy& operator+=(T rhs) 79 { assign(value()+rhs); return *this; } 80 81 /// operator-= for non-const (lvalue) operator -=(const SMatProxy<T> & rhs)82 SMatProxy& operator-=(const SMatProxy<T>& rhs) 83 { assign(value()-rhs); return *this; } 84 85 /// operator-= for const (rvalue) operator -=(T rhs)86 SMatProxy& operator-=(T rhs) 87 { assign(value()-rhs); return *this; } 88 89 /// operator*= for non-const (lvalue) operator *=(const SMatProxy<T> & rhs)90 SMatProxy& operator*=(const SMatProxy<T>& rhs) 91 { assign(value()*rhs); return *this; } 92 93 /// operator*= for const (rvalue) operator *=(T rhs)94 SMatProxy& operator*=(T rhs) 95 { assign(value()*rhs); return *this; } 96 97 private: 98 /// reference to the matrix to which this data belongs 99 SparseMatrix<T>& mySM; 100 101 /// indexes in mySM for this data 102 unsigned int irow, jcol; 103 104 /// get the value of the SparseMatrix at irow,jcol 105 T value(void) const; 106 107 /// assign the SparseMatrix element, used by operator=,+=,etc 108 void assign(T rhs); 109 110 }; // end class SMatProxy 111 112 //--------------------------------------------------------------------------- 113 // implementation of SMatProxy 114 //--------------------------------------------------------------------------- 115 // Default constructor 116 template <class T> SMatProxy(SparseMatrix<T> & sm,unsigned int i,unsigned int j)117 SMatProxy<T>::SMatProxy(SparseMatrix<T>& sm, unsigned int i, unsigned int j) 118 : mySM(sm), irow(i), jcol(j) { } 119 120 //--------------------------------------------------------------------------- 121 // get the value of the SparseMatrix at irow, jcol value(void) const122 template <class T> T SMatProxy<T>::value(void) const 123 { 124 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 125 it = mySM.rowsMap.find(irow); 126 if(it != mySM.rowsMap.end()) 127 return it->second[jcol]; 128 129 return T(); 130 } 131 132 //--------------------------------------------------------------------------- 133 // assignment, used by operator=, operator+=, etc assign(T rhs)134 template <class T> void SMatProxy<T>::assign(T rhs) 135 { 136 typename std::map< unsigned int, SparseVector<T> >::iterator it; 137 it = mySM.rowsMap.find(irow); 138 139 // add a new row? only if row is not there, and rhs is not zero 140 if(T(rhs) != T(0) && it == mySM.rowsMap.end()) { 141 SparseVector<T> SVrow(mySM.ncols); 142 mySM.rowsMap[irow] = SVrow; 143 it = mySM.rowsMap.find(irow); 144 } 145 // data is zero and row is not there...nothing to do 146 else if(it == mySM.rowsMap.end()) return; 147 148 // assign it - let SparseVector::SMatProxy handle r/lvalue 149 // first, do we need to resize the SV? 150 if(it->second.size() < jcol+1) it->second.resize(jcol+1); 151 it->second[jcol] = rhs; 152 153 // if row is now empty, remove it 154 if(it->second.datasize() == 0) 155 mySM.rowsMap.erase(it); 156 } 157 158 //--------------------------------------------------------------------------- 159 // cast operator T() const160 template <class T> SMatProxy<T>::operator T() const 161 { 162 typename std::map< unsigned int, SparseVector<T> >::iterator it; 163 it = mySM.rowsMap.find(irow); 164 if(it == mySM.rowsMap.end()) 165 return T(); 166 else 167 return it->second[jcol]; 168 } 169 170 171 //--------------------------------------------------------------------------- 172 //--------------------------------------------------------------------------- 173 // friends of class SparseMatrix - must declare friends before the template class 174 // min max 175 template <class T> T min(const SparseMatrix<T>& SM); 176 template <class T> T max(const SparseMatrix<T>& SM); 177 template <class T> T minabs(const SparseMatrix<T>& SM); 178 template <class T> T maxabs(const SparseMatrix<T>& SM); 179 // output 180 template <class T> 181 std::ostream& operator<<(std::ostream& os, const SparseMatrix<T>& SM); 182 // transpose 183 template <class T> 184 SparseMatrix<T> transpose(const SparseMatrix<T>& M); 185 // multiplies 186 template <class T> 187 SparseMatrix<T> operator*(const SparseMatrix<T>& L, const SparseMatrix<T>& R); 188 template <class T> 189 SparseVector<T> operator*(const SparseMatrix<T>& L, const SparseVector<T>& V); 190 template <class T> 191 SparseVector<T> operator*(const Matrix<T>& L, const SparseVector<T>& V); 192 template <class T> 193 SparseVector<T> operator*(const SparseMatrix<T>& L, const Vector<T>& V); 194 template <class T> 195 SparseVector<T> operator*(const SparseVector<T>& V, const SparseMatrix<T>& R); 196 template <class T> 197 SparseVector<T> operator*(const SparseVector<T>& V, const Matrix<T>& R); 198 template <class T> 199 SparseVector<T> operator*(const Vector<T>& V, const SparseMatrix<T>& R); 200 template <class T> 201 SparseMatrix<T> operator*(const SparseMatrix<T>& L, const SparseMatrix<T>& R); 202 template <class T> 203 SparseMatrix<T> operator*(const SparseMatrix<T>& L, const Matrix<T>& R); 204 template <class T> 205 SparseMatrix<T> operator*(const Matrix<T>& L, const SparseMatrix<T>& R); 206 // concatenation 207 template <class T> 208 SparseMatrix<T> operator||(const SparseMatrix<T>& L, const Vector<T>& V); 209 template <class T> 210 SparseMatrix<T> operator||(const SparseMatrix<T>& L, const SparseMatrix<T>& R); 211 // addition and subtraction 212 template <class T> SparseMatrix<T> operator-(const SparseMatrix<T>& L, 213 const SparseMatrix<T>& R); 214 template <class T> SparseMatrix<T> operator-(const SparseMatrix<T>& L, 215 const Matrix<T>& R); 216 template <class T> SparseMatrix<T> operator-(const Matrix<T>& L, 217 const SparseMatrix<T>& R); 218 template <class T> SparseMatrix<T> operator+(const SparseMatrix<T>& L, 219 const SparseMatrix<T>& R); 220 template <class T> SparseMatrix<T> operator+(const SparseMatrix<T>& L, 221 const Matrix<T>& R); 222 template <class T> SparseMatrix<T> operator+(const Matrix<T>& L, 223 const SparseMatrix<T>& R); 224 /** inverse (via Gauss-Jordan) 225 * @throw Exception 226 */ 227 template <class T> 228 SparseMatrix<T> inverse(const SparseMatrix<T>& A); 229 /** Cholesky 230 * @throw Exception 231 */ 232 template <class T> 233 SparseMatrix<T> lowerCholesky(const SparseMatrix<T>& A); 234 /** 235 * @throw Exception 236 */ 237 template <class T> 238 SparseMatrix<T> upperCholesky(const SparseMatrix<T>& A); 239 /** inverseLT 240 * @throw Exception 241 */ 242 template <class T> 243 SparseMatrix<T> inverseLT(const SparseMatrix<T>& LT, T *ptrSmall, T *ptrBig); 244 /** 245 * @throw Exception 246 */ 247 template <class T> 248 SparseMatrix<T> inverseViaCholesky(const SparseMatrix<T>& A); 249 250 // special matrices 251 template <class T> 252 SparseMatrix<T> identSparse(const unsigned int dim) throw(); 253 254 /** products MT*M, M*MT, M*C*MT etc 255 * MT * M 256 * @throw Exception 257 */ 258 template <class T> 259 SparseMatrix<T> transposeTimesMatrix(const SparseMatrix<T>& M); 260 /** M * MT 261 * @throw Exception 262 */ 263 template <class T> 264 SparseMatrix<T> matrixTimesTranspose(const SparseMatrix<T>& M); 265 /** diag of P * C * PT 266 * @throw Exception 267 */ 268 template <class T> Vector<T> transformDiag(const SparseMatrix<T>& P, 269 const Matrix<T>& C); 270 271 /** Householder 272 * @throw Exception 273 */ 274 template <class T> 275 SparseMatrix<T> SparseHouseholder(const SparseMatrix<T>& A); 276 277 /** 278 * @throw Exception 279 */ 280 template <class T> 281 void SrifMU(Matrix<T>& R, Vector<T>& Z, SparseMatrix<T>& A, 282 const unsigned int M); 283 /** 284 * @throw Exception 285 */ 286 template <class T> 287 void SrifMU(Matrix<T>& R, Vector<T>& Z, SparseMatrix<T>& P, 288 Vector<T>& D, const unsigned int M); 289 290 //--------------------------------------------------------------------------- 291 /// Class SparseMatrix. This class is designed to present an interface nearly 292 /// identical to class Matrix, but more efficiently handle sparse matrices, in 293 /// which most of the elements are zero. The class stores only non-zero elements; 294 /// using a map of SparseVectors, with key = row index. it also stores a nominal 295 /// dimension - number of rows and columns. The class uses a proxy class, 296 /// SMatProxy, to access elements; this allows rvalues and lvalues to be treated 297 /// separately. 298 /// Notes on speed. The most expensive parts are the Proxy::operator(), then 299 /// find() and lower_bound(). Never use the Proxy stuff within the class, always 300 /// use iterators and assign values to the maps directly. Never assign zeros to 301 /// the maps. See timing and test results in the test program smtest.cpp 302 /// Matrix multiply is orders of magnitude faster. Transpose() is much faster 303 /// than the Matrix version, which is something of a surprise. 304 /// Most time consuming is looping over columns; this is expected since the design 305 /// stores by row. The trick is to re-write the algorithm in terms of the transpose 306 /// of the column-loop matrix, and then apply a transpose(), which is cheap, either 307 /// before starting (when col-loop matrix is input) or after returning (output). 308 /// Then the loops become loops over rows. 309 /// NB. never store zeros in the map, particularly when you are creating the 310 /// matrix and using it at the same time, as in inverseLT(). 311 template <class T> class SparseMatrix 312 { 313 public: 314 // lots of friends 315 /// Proxy needs access to rowsMap 316 friend class SMatProxy<T>; 317 // min max 318 friend T min<T>(const SparseMatrix<T>& SM); 319 friend T max<T>(const SparseMatrix<T>& SM); 320 friend T minabs<T>(const SparseMatrix<T>& SM); 321 friend T maxabs<T>(const SparseMatrix<T>& SM); 322 // stream operator 323 friend std::ostream& operator<< <T>(std::ostream& os, const SparseMatrix<T>& S); 324 // transpose 325 friend SparseMatrix<T> transpose<T>(const SparseMatrix<T>& M); 326 // arithmetic 327 friend SparseMatrix<T> operator-<T>(const SparseMatrix<T>& L, 328 const SparseMatrix<T>& R); 329 friend SparseMatrix<T> operator-<T>(const SparseMatrix<T>& L, 330 const Matrix<T>& R); 331 friend SparseMatrix<T> operator-<T>(const Matrix<T>& L, 332 const SparseMatrix<T>& R); 333 friend SparseMatrix<T> operator+<T>(const SparseMatrix<T>& L, 334 const SparseMatrix<T>& R); 335 friend SparseMatrix<T> operator+<T>(const SparseMatrix<T>& L, 336 const Matrix<T>& R); 337 friend SparseMatrix<T> operator+<T>(const Matrix<T>& L, 338 const SparseMatrix<T>& R); 339 // mulitply 340 friend SparseMatrix<T> operator*<T>(const SparseMatrix<T>& L, 341 const SparseMatrix<T>& R); 342 friend SparseVector<T> operator*<T>(const SparseMatrix<T>& L, 343 const SparseVector<T>& V); 344 friend SparseVector<T> operator*<T>(const Matrix<T>& L, 345 const SparseVector<T>& V); 346 friend SparseVector<T> operator*<T>(const SparseMatrix<T>& L, 347 const Vector<T>& V); 348 friend SparseVector<T> operator*<T>(const SparseVector<T>& V, 349 const SparseMatrix<T>& R); 350 friend SparseVector<T> operator*<T>(const SparseVector<T>& V, 351 const Matrix<T>& R); 352 friend SparseVector<T> operator*<T>(const Vector<T>& V, 353 const SparseMatrix<T>& R); 354 friend SparseMatrix<T> operator*<T>(const SparseMatrix<T>& L, 355 const SparseMatrix<T>& R); 356 friend SparseMatrix<T> operator*<T>(const SparseMatrix<T>& L, 357 const Matrix<T>& R); 358 friend SparseMatrix<T> operator*<T>(const Matrix<T>& L, 359 const SparseMatrix<T>& R); 360 // concatenation 361 friend SparseMatrix<T> operator||<T>(const SparseMatrix<T>& L, 362 const Vector<T>& V); 363 friend SparseMatrix<T> operator||<T>(const SparseMatrix<T>& L, 364 const SparseMatrix<T>& R); 365 // inverse (via Gauss-Jordan) 366 friend SparseMatrix<T> inverse<T>(const SparseMatrix<T>& A); 367 // Cholesky 368 friend SparseMatrix<T> lowerCholesky<T>(const SparseMatrix<T>& A); 369 friend SparseMatrix<T> upperCholesky<T>(const SparseMatrix<T>& A); 370 friend SparseMatrix<T> inverseLT<T>(const SparseMatrix<T>& LT, 371 T *ptrSmall, T *ptrBig); 372 friend SparseMatrix<T> inverseViaCholesky<T>(const SparseMatrix<T>& A); 373 // special matrices 374 friend SparseMatrix<T> identSparse<T>(const unsigned int dim) throw(); 375 376 // MT * M 377 friend SparseMatrix<T> transposeTimesMatrix<T>(const SparseMatrix<T>& M); 378 // M * MT 379 friend SparseMatrix<T> matrixTimesTranspose<T>(const SparseMatrix<T>& M); 380 // diag of P * C * PT 381 friend Vector<T> transformDiag<T>(const SparseMatrix<T>& P, 382 const Matrix<T>& C); 383 friend SparseMatrix<T> SparseHouseholder<T>(const SparseMatrix<T>& A); 384 friend void SrifMU<T>(Matrix<T>& R, Vector<T>& Z, SparseMatrix<T>& A, 385 const unsigned int M); 386 friend void SrifMU<T>(Matrix<T>& R, Vector<T>& Z, SparseMatrix<T>& P, 387 Vector<T>& D, const unsigned int M); 388 389 /// empty constructor SparseMatrix(void)390 SparseMatrix(void) : nrows(0), ncols(0) { } 391 392 /// constructor with dimensions SparseMatrix(unsigned int r,unsigned int c)393 SparseMatrix(unsigned int r, unsigned int c) : nrows(r), ncols(c) { } 394 395 /// sub-matrix constructor 396 /// @param SV SparseVector to copy 397 /// @param ind starting index for the copy 398 /// @param n length of new SparseVector 399 SparseMatrix(const SparseMatrix<T>& SM, 400 const unsigned int& rind, const unsigned int& cind, 401 const unsigned int& rnum, const unsigned int& cnum); 402 403 /// constructor from regular Matrix<T> 404 SparseMatrix(const Matrix<T>& M); 405 406 // TD watch for unintended consequences - cast to Matrix to use some Matrix::fun 407 /// cast to Matrix or implicit conversion to Matrix<T> 408 operator Matrix<T>() const; 409 410 /// get number of rows - of the real Matrix, not the data array rows(void) const411 inline unsigned int rows(void) const { return nrows; } 412 413 /// get number of columns - of the real Matrix, not the data array cols(void) const414 inline unsigned int cols(void) const { return ncols; } 415 416 /// size of matrix = rows()*cols() size(void) const417 inline unsigned int size(void) const { return nrows*ncols; } 418 419 /// datasize - number of non-zero data datasize(void) const420 inline unsigned int datasize(void) const 421 { 422 unsigned int n(0); 423 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 424 it = rowsMap.begin(); 425 for( ; it != rowsMap.end(); ++it) 426 n += it->second.datasize(); 427 return n; 428 } 429 430 /// is this SM empty? NB may have to call zeroize() to get a yes. isEmpty(void) const431 inline bool isEmpty(void) const 432 { return (rowsMap.begin() == rowsMap.end()); } 433 434 /// density - ratio of number of non-zero element to size() density(void) const435 inline double density(void) const 436 { return (double(datasize())/double(size())); } 437 438 /// resize - only changes len and removes elements if necessary resize(const unsigned int newrows,const unsigned int newcols)439 void resize(const unsigned int newrows, const unsigned int newcols) 440 { 441 typename std::map< unsigned int, SparseVector<T> >::iterator it; 442 if(newrows < nrows) { 443 // lower_bound returns it for first key >= newrows 444 it = rowsMap.lower_bound(newrows); 445 rowsMap.erase(it,rowsMap.end()); 446 } 447 if(newcols < ncols) 448 for(it=rowsMap.begin(); it != rowsMap.end(); ++it) 449 it->second.resize(newcols); 450 451 nrows = newrows; 452 ncols = newcols; 453 } 454 455 /// clear - set all data to 0 (i.e. remove all data); leave dimensions alone clear(void)456 inline void clear(void) 457 { rowsMap.clear(); } 458 459 /// zeroize - remove elements that are less than tolerance in abs value 460 /// NB. This routine is called only by the user - routines defined here do not 461 /// zeroize, as there is no way to appropriately choose a tolerance. 462 /// The default tolerance for this routine is SparseVector<T>::zeroTolerance. 463 /// The caller may want to consider a tolerance related to SM.maxabs(). 464 void zeroize(const T tol=static_cast<T>(SparseVector<T>::zeroTolerance)); 465 466 /// true if the element (i,j) is non-zero isFilled(const unsigned int i,const unsigned int j)467 inline bool isFilled(const unsigned int i, const unsigned int j) 468 { 469 typename std::map< unsigned int, SparseVector<T> >::iterator it; 470 it = rowsMap.find(i); 471 return (it != rowsMap.end() && it->second.isFilled(j)); 472 } 473 474 // operators ---------------------------------------------------- 475 /// operator() for const, but SMatProxy does all the work operator ()(unsigned int i,unsigned int j) const476 const SMatProxy<T> operator()(unsigned int i, unsigned int j) const 477 { 478 #ifdef RANGECHECK 479 if(i >= nrows) GPSTK_THROW(Exception("row index out of range")); 480 if(j >= ncols) GPSTK_THROW(Exception("col index out of range")); 481 #endif 482 return SMatProxy<T>(const_cast<SparseMatrix&>(*this), i, j); 483 } 484 485 /// operator() for non-const, but SMatProxy does all the work operator ()(unsigned int i,unsigned int j)486 SMatProxy<T> operator()(unsigned int i, unsigned int j) 487 { 488 #ifdef RANGECHECK 489 if(i >= nrows) GPSTK_THROW(Exception("row index out of range")); 490 if(j >= ncols) GPSTK_THROW(Exception("col index out of range")); 491 #endif 492 return SMatProxy<T>(*this, i, j); 493 } 494 495 // output ------------------------------------------------------- 496 /// Dump only non-zero values, with indexes (i,value) dump(const int p=3,bool dosci=false) const497 std::string dump(const int p=3, bool dosci=false) const 498 { 499 std::ostringstream oss; 500 size_t i; 501 oss << "dim(" << nrows << "," << ncols << "), size " << size() 502 << ", datasize " << datasize() << " :"; 503 oss << (dosci ? std::scientific : std::fixed) << std::setprecision(p); 504 505 // loop over rows 506 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 507 it = rowsMap.begin(); 508 if(it == rowsMap.end()) { oss << " empty"; return oss.str(); } 509 510 for( ; it != rowsMap.end(); ++it) { 511 oss << "\n row " << it->first << ": " << it->second.dump(p,dosci); 512 } 513 514 return oss.str(); 515 } 516 517 /// Convert to "dump-able" form : 3 parallel vectors; rows, cols, values flatten(std::vector<unsigned int> & rows,std::vector<unsigned int> & cols,std::vector<T> & values) const518 void flatten(std::vector<unsigned int>& rows, 519 std::vector<unsigned int>& cols, 520 std::vector<T>& values) 521 const 522 { 523 rows.clear(); cols.clear(); values.clear(); 524 525 // loop over rows, then columns 526 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 527 typename std::map<unsigned int, T>::const_iterator jt; 528 for(it = rowsMap.begin(); it != rowsMap.end(); ++it) { 529 unsigned int row(it->first); 530 for(jt = it->second.vecMap.begin(); jt != it->second.vecMap.end(); ++jt) { 531 rows.push_back(row); 532 cols.push_back(jt->first); 533 values.push_back(jt->second); 534 } 535 } 536 } 537 538 /// Minimum element - return 0 if empty 539 // arithmetic and other operators 540 SparseMatrix<T>& operator-=(const SparseMatrix<T>& SM); 541 SparseMatrix<T>& operator-=(const Matrix<T>& SM); 542 SparseMatrix<T>& operator+=(const SparseMatrix<T>& SM); 543 SparseMatrix<T>& operator+=(const Matrix<T>& SM); 544 SparseMatrix<T>& operator*=(const T& value); 545 /** 546 * @throw Exception 547 */ 548 SparseMatrix<T>& operator/=(const T& value); 549 550 // unary minus operator -() const551 SparseMatrix<T> operator-() const 552 { 553 SparseMatrix<T> toRet(*this); 554 typename std::map< unsigned int, SparseVector<T> >::iterator it; 555 typename std::map<unsigned int, T>::iterator jt; 556 for(it = toRet.rowsMap.begin(); it != toRet.rowsMap.end(); ++it) { 557 for(jt = it->second.vecMap.begin(); jt != it->second.vecMap.end(); ++jt) 558 jt->second = -jt->second; 559 } 560 return toRet; 561 } 562 563 /// return row i of this SparseMatrix as a SparseVector 564 SparseVector<T> rowCopy(const unsigned int i) const; 565 566 /// return col j of this SparseMatrix as a SparseVector 567 SparseVector<T> colCopy(const unsigned int j) const; 568 569 /// return diagonal of this SparseMatrix as a SparseVector 570 SparseVector<T> diagCopy(void) const; 571 572 /// swap rows of this SparseMatrix 573 void swapRows(const unsigned int& ii, const unsigned int& jj); 574 575 /// swap columns of this SparseMatrix 576 void swapCols(const unsigned int ii, const unsigned int jj); 577 578 private: 579 /// dimensions of the "real" matrix (not the number of data stored) 580 unsigned int nrows, ncols; 581 582 /// map of row index, row SparseVector 583 std::map< unsigned int, SparseVector<T> > rowsMap; 584 585 // TD ? obsolete this by always using transpose when you must loop over columns 586 /// private function to build a "column map" for this matrix, 587 /// containing vectors (of row-indexes) for each column. 588 /// colMap[column index] = vector of all row indexes, in ascending order columnMap(void) const589 std::map< unsigned int, std::vector<unsigned int> > columnMap(void) const 590 { 591 unsigned int j,k; 592 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 593 typename std::map< unsigned int, std::vector<unsigned int> > colMap; 594 typename std::map< unsigned int, std::vector<unsigned int> >::iterator jt; 595 596 // loop over rows, then columns 597 for(it = rowsMap.begin(); it != rowsMap.end(); ++it) { 598 599 // get all the column indexes for this row 600 std::vector<unsigned int> colIndexes = it->second.getIndexes(); 601 602 // loop over columns, adding each to the colMap 603 for(k=0; k<colIndexes.size(); k++) { 604 j = colIndexes[k]; 605 jt = colMap.find(j); 606 if(jt == colMap.end()) { // add a vector for this column 607 std::vector<unsigned int> jvec; 608 colMap[j] = jvec; 609 jt = colMap.find(j); 610 } 611 jt->second.push_back(it->first); // add row index to this col-vector 612 } 613 } 614 615 //std::ostringstream oss; 616 //for(jt=colMap.begin(); jt!=colMap.end(); ++jt) { 617 // oss << " col " << jt->first << " ["; 618 // for(k=0; k<jt->second.size(); k++) 619 // oss << " " << jt->second[k]; 620 // oss << "]\n"; 621 //} 622 //std::cout << "Column map:\n" << oss.str(); 623 624 return colMap; 625 } 626 627 }; // end class SparseMatrix 628 629 630 //--------------------------------------------------------------------------- 631 // implementation of SparseMatrix 632 //--------------------------------------------------------------------------- 633 // submatrix constructor SparseMatrix(const SparseMatrix<T> & SM,const unsigned int & rind,const unsigned int & cind,const unsigned int & rnum,const unsigned int & cnum)634 template <class T> SparseMatrix<T>::SparseMatrix(const SparseMatrix<T>& SM, 635 const unsigned int& rind, const unsigned int& cind, 636 const unsigned int& rnum, const unsigned int& cnum) 637 { 638 if(rnum == 0 || cnum == 0 || rind+rnum > SM.nrows || cind+cnum > SM.ncols) 639 GPSTK_THROW(Exception("Invalid input submatrix c'tor - out of range")); 640 641 nrows = rnum; 642 ncols = cnum; 643 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 644 for(it = SM.rowsMap.begin(); it != SM.rowsMap.end(); ++it) { 645 if(it->first < rind) continue; // skip rows before rind 646 if(it->first > rind+rnum) break; // done with rows 647 SparseVector<T> SV(it->second,cind,cnum); // get sub-vector 648 if(!SV.isEmpty()) rowsMap[it->first] = SV; // add it 649 } 650 } 651 652 // constructor from regular Matrix<T> SparseMatrix(const Matrix<T> & M)653 template <class T> SparseMatrix<T>::SparseMatrix(const Matrix<T>& M) 654 { 655 unsigned i,j; 656 nrows = M.rows(); 657 ncols = M.cols(); 658 for(i=0; i<nrows; i++) { 659 bool haverow(false); // rather than rowsMap.find() every time 660 for(j=0; j<ncols; j++) { 661 if(M(i,j) == T(0)) continue; // nothing to do - element(i,j) is zero 662 663 // non-zero, must add it 664 if(!haverow) { // add row i 665 SparseVector<T> SVrow(ncols); // 'real' length ncols 666 rowsMap[i] = SVrow; 667 haverow = true; 668 } 669 rowsMap[i].vecMap[j] = M(i,j); 670 } 671 } 672 } 673 674 /// cast to Matrix<T> operator Matrix<T>() const675 template <class T> SparseMatrix<T>::operator Matrix<T>() const 676 { 677 Matrix<T> toRet(nrows,ncols,T(0)); 678 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 679 typename std::map< unsigned int, T >::const_iterator jt; 680 for(it = rowsMap.begin(); it != rowsMap.end(); ++it) { 681 for(jt = it->second.vecMap.begin(); jt != it->second.vecMap.end(); ++jt) { 682 toRet(it->first,jt->first) = jt->second; 683 } 684 } 685 686 return toRet; 687 } 688 689 /// zeroize - remove elements that are less than tolerance in abs value zeroize(const T tol)690 template <class T> void SparseMatrix<T>::zeroize(const T tol) 691 { 692 std::vector<unsigned int> toDelete; // row indexes 693 typename std::map< unsigned int, SparseVector<T> >::iterator it; 694 for(it = rowsMap.begin(); it != rowsMap.end(); ++it) { 695 it->second.zeroize(tol); 696 697 // remove row if its empty - but not inside the iteration loop 698 if(it->second.datasize() == 0) 699 toDelete.push_back(it->first); 700 } 701 702 // now remove the marked rows 703 for(unsigned int i=0; i<toDelete.size(); i++) { 704 rowsMap.erase(toDelete[i]); 705 } 706 } 707 708 /// transpose transpose(const SparseMatrix<T> & M)709 template <class T> SparseMatrix<T> transpose(const SparseMatrix<T>& M) 710 { 711 SparseMatrix<T> toRet(M.cols(),M.rows()); 712 713 // loop over rows of M = columns of toRet - faster 714 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 715 typename std::map< unsigned int, SparseVector<T> >::iterator jt; 716 for(it = M.rowsMap.begin(); it != M.rowsMap.end(); ++it) { 717 for(unsigned int j=0; j<M.cols(); j++) { 718 if(it->second.isFilled(j)) { // M(i,j) 719 jt = toRet.rowsMap.find(j); 720 if(jt == toRet.rowsMap.end()) { // add the row 721 SparseVector<T> rowSV(M.rows()); 722 toRet.rowsMap[j] = rowSV; 723 jt = toRet.rowsMap.find(j); 724 } 725 jt->second.vecMap[it->first] = it->second[j]; 726 } 727 } 728 } 729 730 return toRet; 731 } 732 733 /// Maximum element - return 0 if empty min(const SparseMatrix<T> & SM)734 template <class T> T min(const SparseMatrix<T>& SM) 735 { 736 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 737 it = SM.rowsMap.begin(); 738 if(it == SM.rowsMap.end()) return T(0); 739 740 T value(min(it->second)); // first non-zero row 741 for(++it ; it != SM.rowsMap.end(); ++it ) { // other rows 742 T rowval(min(it->second)); 743 if(rowval < value) value = rowval; 744 } 745 746 return value; 747 } 748 749 /// Maximum element - return 0 if empty max(const SparseMatrix<T> & SM)750 template <class T> T max(const SparseMatrix<T>& SM) 751 { 752 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 753 it = SM.rowsMap.begin(); 754 if(it == SM.rowsMap.end()) return T(0); 755 756 T value(max(it->second)); // first non-zero row 757 for(++it ; it != SM.rowsMap.end(); ++it ) { // other rows 758 T rowval(max(it->second)); 759 if(rowval > value) value = rowval; 760 } 761 762 return value; 763 } 764 765 /// Minimum absolute value - return 0 if empty minabs(const SparseMatrix<T> & SM)766 template <class T> T minabs(const SparseMatrix<T>& SM) 767 { 768 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 769 it = SM.rowsMap.begin(); 770 if(it == SM.rowsMap.end()) return T(0); 771 772 T value(minabs(it->second)); // first non-zero row 773 for(++it ; it != SM.rowsMap.end(); ++it ) { // other rows 774 T rowval(minabs(it->second)); 775 if(rowval < value) value = rowval; 776 } 777 778 return value; 779 } 780 781 /// Maximum absolute value - return 0 if empty maxabs(const SparseMatrix<T> & SM)782 template <class T> T maxabs(const SparseMatrix<T>& SM) 783 { 784 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 785 it = SM.rowsMap.begin(); 786 if(it == SM.rowsMap.end()) return T(0); 787 788 T value(maxabs(it->second)); // first non-zero row 789 for(++it ; it != SM.rowsMap.end(); ++it ) { // other rows 790 T rowval(maxabs(it->second)); 791 if(rowval > value) value = rowval; 792 } 793 794 return value; 795 } 796 797 //--------------------------------------------------------------------------------- 798 /// stream output operator 799 template <class T> operator <<(std::ostream & os,const SparseMatrix<T> & SM)800 std::ostream& operator<<(std::ostream& os, const SparseMatrix<T>& SM) 801 { 802 std::ofstream savefmt; 803 savefmt.copyfmt(os); 804 805 unsigned int i, j; // the "real" vector row and column index 806 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 807 808 it = SM.rowsMap.begin(); 809 //if(it == SM.rowsMap.end()) { os << "empty"; return os; } 810 811 for(i=0; i<SM.nrows; i++) { 812 if(it != SM.rowsMap.end() && i == it->first) { 813 os << std::setw(1) << ' '; os.copyfmt(savefmt); 814 os << it->second; // write the whole row 815 ++it; 816 } 817 else { 818 for(j=0; j<SM.ncols; j++) { // write a row of '0' 819 os << std::setw(1) << ' '; os.copyfmt(savefmt); 820 os << "0"; 821 } 822 } 823 if(i < SM.nrows-1) os << "\n"; 824 } 825 826 return os; 827 } 828 829 //--------------------------------------------------------------------------- 830 // SparseMatrix operators 831 //--------------------------------------------------------------------------- 832 833 /// Matrix,Vector multiply: SparseVector = SparseMatrix * SparseVector 834 template <class T> operator *(const SparseMatrix<T> & L,const SparseVector<T> & V)835 SparseVector<T> operator*(const SparseMatrix<T>& L, const SparseVector<T>& V) 836 { 837 try { 838 if(L.cols() != V.size()) 839 GPSTK_THROW(Exception("Incompatible dimensions op*(SM,SV)")); 840 841 SparseVector<T> retSV(L.rows()); 842 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 843 844 // loop over rows of L = rows of answer 845 for(it = L.rowsMap.begin(); it != L.rowsMap.end(); ++it) 846 retSV.vecMap[it->first] = dot(it->second,V); 847 848 retSV.zeroize(T(0)); 849 return retSV; 850 } 851 catch(Exception& e) { GPSTK_RETHROW(e); } 852 } 853 854 /// Matrix,Vector multiply: SparseVector = Matrix * SparseVector 855 template <class T> operator *(const Matrix<T> & L,const SparseVector<T> & V)856 SparseVector<T> operator*(const Matrix<T>& L, const SparseVector<T>& V) 857 { 858 try { 859 if(L.cols() != V.size()) 860 GPSTK_THROW(Exception("Incompatible dimensions op*(M,SV)")); 861 862 SparseVector<T> retSV(L.rows()); 863 864 // loop over rows of L = rows of answer 865 for(unsigned int i=0; i<L.rows(); i++) { 866 T sum(0); 867 // loop over elements of V 868 typename std::map<unsigned int, T>::const_iterator it; 869 for(it = V.vecMap.begin(); it != V.vecMap.end(); ++it) 870 sum += L(i,it->first) * it->second; 871 retSV.vecMap[i] = sum; 872 } 873 874 retSV.zeroize(T(0)); 875 return retSV; 876 } 877 catch(Exception& e) { GPSTK_RETHROW(e); } 878 } 879 880 /// Matrix,Vector multiply: SparseVector = SparseMatrix * Vector 881 template <class T> operator *(const SparseMatrix<T> & L,const Vector<T> & V)882 SparseVector<T> operator*(const SparseMatrix<T>& L, const Vector<T>& V) 883 { 884 try { 885 if(L.cols() != V.size()) 886 GPSTK_THROW(Exception("Incompatible dimensions op*(SM,V)")); 887 888 SparseVector<T> retSV(L.rows()); 889 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 890 891 // loop over rows of L = rows of answer 892 for(it = L.rowsMap.begin(); it != L.rowsMap.end(); ++it) 893 retSV.vecMap[it->first] = dot(it->second,V); 894 895 retSV.zeroize(T(0)); 896 return retSV; 897 } 898 catch(Exception& e) { GPSTK_RETHROW(e); } 899 } 900 901 /// Vector,Matrix multiply: SparseVector = SparseVector * SparseMatrix 902 template <class T> operator *(const SparseVector<T> & V,const SparseMatrix<T> & R)903 SparseVector<T> operator*(const SparseVector<T>& V, const SparseMatrix<T>& R) 904 { 905 if(V.size() != R.rows()) 906 GPSTK_THROW(Exception("Incompatible dimensions op*(SV,SM)")); 907 908 SparseVector<T> retSV(R.cols()); 909 910 // first build a "column map" for R - vectors (of row-index) for each column 911 // colMap[column index] = vector of all row indexes, in ascending order 912 std::map< unsigned int, std::vector<unsigned int> >::iterator jt; 913 std::map< unsigned int, std::vector<unsigned int> > colMap = R.columnMap(); 914 915 // loop over columns of R = elements of answer 916 for(jt = colMap.begin(); jt != colMap.end(); ++jt) { 917 T sum(0); 918 // loop over rows of R and elements of V 919 for(unsigned int k=0; k<jt->second.size(); k++) 920 if(V.isFilled(jt->second[k])) 921 sum += V[jt->second[k]] * R(k,jt->first); 922 if(sum != T(0)) retSV.vecMap[jt->first] = sum; 923 } 924 925 return retSV; 926 } 927 928 /// Vector,Matrix multiply: SparseVector = SparseVector * Matrix 929 template <class T> operator *(const SparseVector<T> & V,const Matrix<T> & R)930 SparseVector<T> operator*(const SparseVector<T>& V, const Matrix<T>& R) 931 { 932 try { 933 if(V.size() != R.rows()) 934 GPSTK_THROW(Exception("Incompatible dimensions op*(SV,M)")); 935 936 T sum; 937 SparseVector<T> retSV(R.cols()); 938 939 // loop over columns of R = elements of answer 940 for(unsigned int j=0; j<R.cols(); j++) { 941 // copy out the whole column as a Vector 942 Vector<T> colR = R.colCopy(j); 943 sum = dot(colR,V); 944 if(sum != T(0)) retSV.vecMap[j] = sum; 945 } 946 947 return retSV; 948 } 949 catch(Exception& e) { GPSTK_RETHROW(e); } 950 } 951 952 /// Vector,Matrix multiply: SparseVector = Vector * SparseMatrix 953 template <class T> operator *(const Vector<T> & V,const SparseMatrix<T> & R)954 SparseVector<T> operator*(const Vector<T>& V, const SparseMatrix<T>& R) 955 { 956 if(V.size() != R.rows()) 957 GPSTK_THROW(Exception("Incompatible dimensions op*(V,SM)")); 958 959 SparseVector<T> retSV(R.cols()); 960 961 // first build a "column map" for R - vectors (of row-index) for each column 962 // colMap[column index] = vector of all row indexes, in ascending order 963 std::map< unsigned int, std::vector<unsigned int> >::iterator jt; 964 std::map< unsigned int, std::vector<unsigned int> > colMap = R.columnMap(); 965 966 // loop over columns of R = elements of answer 967 for(jt = colMap.begin(); jt != colMap.end(); ++jt) { 968 T sum(0); 969 // loop over rows of R and elements of V 970 for(unsigned int k=0; k<jt->second.size(); k++) 971 sum += V[jt->second[k]] * R(jt->second[k],jt->first); 972 if(sum != T(0)) retSV.vecMap[jt->first] = sum; 973 } 974 975 return retSV; 976 } 977 978 /// Matrix multiply: SparseMatrix = SparseMatrix * SparseMatrix 979 template <class T> operator *(const SparseMatrix<T> & L,const SparseMatrix<T> & R)980 SparseMatrix<T> operator*(const SparseMatrix<T>& L, const SparseMatrix<T>& R) 981 { 982 if(L.cols() != R.rows()) 983 GPSTK_THROW(Exception("Incompatible dimensions op*(SM,SM)")); 984 985 const unsigned int nr(L.rows()), nc(R.cols()); 986 SparseMatrix<T> retSM(nr,nc); // empty but with correct dimen. 987 const SparseMatrix<T> RT(transpose(R)); // work with transpose 988 typename std::map< unsigned int, SparseVector<T> >::const_iterator it, jt; 989 990 // loop over rows of L = rows of retSM 991 for(it = L.rowsMap.begin(); it != L.rowsMap.end(); ++it) { 992 bool haveRow(false); // use to create the row 993 // loop over rows of RT == columns of R 994 for(jt = RT.rowsMap.begin(); jt != RT.rowsMap.end(); ++jt) { 995 // answer(i,j) = dot product of Lrow and RTrow==Rcol 996 T d(dot(it->second,jt->second)); 997 if(d != T(0)) { 998 if(!haveRow) { 999 SparseVector<T> row(nc); 1000 retSM.rowsMap[it->first] = row; 1001 haveRow = true; 1002 } 1003 retSM.rowsMap[it->first].vecMap[jt->first] = d; 1004 } 1005 } 1006 } 1007 1008 return retSM; 1009 } 1010 1011 /// Matrix multiply: SparseMatrix = SparseMatrix * Matrix 1012 template <class T> operator *(const SparseMatrix<T> & L,const Matrix<T> & R)1013 SparseMatrix<T> operator*(const SparseMatrix<T>& L, const Matrix<T>& R) 1014 { 1015 try { 1016 if(L.cols() != R.rows()) 1017 GPSTK_THROW(Exception("Incompatible dimensions op*(SM,M)")); 1018 1019 const unsigned int nr(L.rows()), nc(R.cols()); 1020 SparseMatrix<T> retSM(nr,nc); 1021 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 1022 1023 // loop over rows of L = rows of answer 1024 for(it = L.rowsMap.begin(); it != L.rowsMap.end(); ++it) { 1025 bool haveRow(false); // use to create the row 1026 // loop over columns of R = cols of answer 1027 for(unsigned int j=0; j<R.cols(); j++) { 1028 Vector<T> colR(R.colCopy(j)); 1029 T d(dot(it->second,colR)); 1030 if(d != T(0)) { 1031 if(!haveRow) { 1032 SparseVector<T> row(nc); 1033 retSM.rowsMap[it->first] = row; 1034 haveRow = true; 1035 } 1036 retSM.rowsMap[it->first].vecMap[j] = d; 1037 } 1038 } 1039 } 1040 1041 return retSM; 1042 } 1043 catch(Exception& e) { GPSTK_RETHROW(e); } 1044 } 1045 1046 /// Matrix multiply: SparseMatrix = Matrix * SparseMatrix 1047 template <class T> operator *(const Matrix<T> & L,const SparseMatrix<T> & R)1048 SparseMatrix<T> operator*(const Matrix<T>& L, const SparseMatrix<T>& R) 1049 { 1050 if(L.cols() != R.rows()) 1051 GPSTK_THROW(Exception("Incompatible dimensions op*(M,SM)")); 1052 1053 const unsigned int nr(L.rows()), nc(R.cols()); 1054 SparseMatrix<T> retSM(nr,nc); 1055 const SparseMatrix<T> RT(transpose(R)); // work with transpose 1056 typename std::map< unsigned int, SparseVector<T> >::const_iterator jt; 1057 1058 // loop over rows of L = rows of retSM 1059 for(unsigned int i=0; i < nr; i++) { 1060 bool haveRow(false); // use to create the row in retSM 1061 const Vector<T> rowL(L.rowCopy(i)); // copy out the row in L 1062 1063 // loop over rows of RT == columns of R 1064 for(jt = RT.rowsMap.begin(); jt != RT.rowsMap.end(); ++jt) { 1065 // answer(i,j) = dot product of Lrow and RTrow==Rcol 1066 T d(dot(rowL,jt->second)); 1067 if(d != T(0)) { 1068 if(!haveRow) { 1069 SparseVector<T> row(nc); 1070 retSM.rowsMap[i] = row; 1071 haveRow = true; 1072 } 1073 retSM.rowsMap[i].vecMap[jt->first] = d; 1074 } 1075 } 1076 } 1077 1078 return retSM; 1079 } 1080 1081 //--------------------------------------------------------------------------- 1082 //--------------------------------------------------------------------------- 1083 /// Concatenation SparseMatrix || Vector TD the rest of them... 1084 template <class T> operator ||(const SparseMatrix<T> & L,const Vector<T> & V)1085 SparseMatrix<T> operator||(const SparseMatrix<T>& L, const Vector<T>& V) 1086 { 1087 if(L.rows() != V.size()) 1088 GPSTK_THROW(Exception("Incompatible dimensions op||(SM,V)")); 1089 1090 SparseMatrix<T> toRet(L); 1091 toRet.ncols++; 1092 1093 unsigned int i,n(toRet.ncols-1); 1094 typename std::map< unsigned int, SparseVector<T> >::iterator jt; 1095 for(i=0; i<V.size(); i++) { 1096 if(V(i) != T(0)) { // add it 1097 jt = toRet.rowsMap.find(i); // find the row 1098 if(jt == toRet.rowsMap.end()) { // add the row 1099 SparseVector<T> V(toRet.ncols); 1100 toRet.rowsMap[i] = V; 1101 } 1102 toRet.rowsMap[i].vecMap[n] = V(i); 1103 } 1104 } 1105 return toRet; 1106 } 1107 1108 //--------------------------------------------------------------------------- 1109 template <class T> operator ||(const SparseMatrix<T> & L,const SparseMatrix<T> & R)1110 SparseMatrix<T> operator||(const SparseMatrix<T>& L, const SparseMatrix<T>& R) 1111 { 1112 if(L.rows() != R.rows()) 1113 GPSTK_THROW(Exception("Incompatible dimensions op||(SM,SM)")); 1114 1115 SparseMatrix<T> toRet(L); 1116 toRet.ncols += R.ncols; 1117 1118 unsigned int i, N(L.ncols); 1119 typename std::map< unsigned int, SparseVector<T> >::iterator it; 1120 typename std::map< unsigned int, SparseVector<T> >::const_iterator jt; 1121 it = toRet.rowsMap.begin(); 1122 jt = R.rowsMap.begin(); 1123 while(it != toRet.rowsMap.end() && jt != R.rowsMap.end()) { 1124 if(it->first < jt->first) { // R has no row here - do nothing 1125 it->second.len += N; 1126 ++it; 1127 } 1128 else if(it->first > jt->first) { // R has a row where toRet does not 1129 toRet.rowsMap[jt->first] = jt->second; 1130 toRet.rowsMap[jt->first].len += N; 1131 ++jt; 1132 } 1133 else { // equal indexes - both have rows 1134 it->second.len += N; 1135 typename std::map< unsigned int, T >::const_iterator vt; 1136 for(vt = jt->second.vecMap.begin(); vt != jt->second.vecMap.end(); ++vt) { 1137 toRet.rowsMap[it->first].vecMap[N+vt->first] = vt->second; 1138 } 1139 ++it; 1140 ++jt; 1141 } 1142 } 1143 1144 return toRet; 1145 } 1146 1147 //--------------------------------------------------------------------------- 1148 //--------------------------------------------------------------------------- 1149 /// Matrix subtraction: SparseMatrix -= SparseMatrix 1150 template <class T> operator -=(const SparseMatrix<T> & SM)1151 SparseMatrix<T>& SparseMatrix<T>::operator-=(const SparseMatrix<T>& SM) 1152 { 1153 if(SM.cols() != cols() || SM.rows() != rows()) 1154 GPSTK_THROW(Exception("Incompatible dimensions op-=(SM)")); 1155 //std::cout << "SM::op-=(SM)" << std::endl; 1156 1157 // loop over rows of SM 1158 typename std::map< unsigned int, SparseVector<T> >::const_iterator sit; 1159 for(sit = SM.rowsMap.begin(); sit != SM.rowsMap.end(); ++sit) { 1160 // find same row in this 1161 if(rowsMap.find(sit->first) == rowsMap.end()) 1162 rowsMap[sit->first] = -sit->second; 1163 else 1164 rowsMap[sit->first] -= sit->second; 1165 } 1166 1167 zeroize(T(0)); 1168 return *this; 1169 } 1170 1171 /// Matrix subtraction: SparseMatrix -= Matrix 1172 template <class T> operator -=(const Matrix<T> & M)1173 SparseMatrix<T>& SparseMatrix<T>::operator-=(const Matrix<T>& M) 1174 { 1175 if(M.cols() != cols() || M.rows() != rows()) 1176 GPSTK_THROW(Exception("Incompatible dimensions op-=(M)")); 1177 //std::cout << "SM::op-=(M)" << std::endl; 1178 1179 // loop over rows of M 1180 for(unsigned int i=0; i<M.rows(); i++) { 1181 Vector<T> rowV(M.rowCopy(i)); 1182 if(rowsMap.find(i) == rowsMap.end()) { 1183 SparseVector<T> SV(rowV); 1184 rowsMap[i] = -SV; 1185 } 1186 else 1187 rowsMap[i] -= rowV; 1188 } 1189 1190 zeroize(T(0)); 1191 return *this; 1192 } 1193 1194 /// Matrix subtraction: SparseMatrix = SparseMatrix - SparseMatrix 1195 template <class T> operator -(const SparseMatrix<T> & L,const SparseMatrix<T> & R)1196 SparseMatrix<T> operator-(const SparseMatrix<T>& L, const SparseMatrix<T>& R) 1197 { 1198 if(L.cols() != R.cols() || L.rows() != R.rows()) 1199 GPSTK_THROW(Exception("Incompatible dimensions op-(SM,SM)")); 1200 //std::cout << "SM::op-(SM,SM)" << std::endl; 1201 1202 SparseMatrix<T> retSM(L); 1203 retSM -= R; // will zeroize(T(0)) 1204 1205 return retSM; 1206 } 1207 1208 /// Matrix subtraction: SparseMatrix = SparseMatrix - Matrix 1209 template <class T> operator -(const SparseMatrix<T> & L,const Matrix<T> & R)1210 SparseMatrix<T> operator-(const SparseMatrix<T>& L, const Matrix<T>& R) 1211 { 1212 if(L.cols() != R.cols() || L.rows() != R.rows()) 1213 GPSTK_THROW(Exception("Incompatible dimensions op-(SM,M)")); 1214 //std::cout << "SM::op-(SM,M)" << std::endl; 1215 1216 SparseMatrix<T> retSM(L); 1217 retSM -= R; // will zeroize(T(0)) 1218 1219 return retSM; 1220 } 1221 1222 /// Matrix subtraction: SparseMatrix = Matrix - SparseMatrix 1223 template <class T> operator -(const Matrix<T> & L,const SparseMatrix<T> & R)1224 SparseMatrix<T> operator-(const Matrix<T>& L, const SparseMatrix<T>& R) 1225 { 1226 if(L.cols() != R.cols() || L.rows() != R.rows()) 1227 GPSTK_THROW(Exception("Incompatible dimensions op-(M,SM)")); 1228 //std::cout << "SM::op-(M,SM)" << std::endl; 1229 1230 SparseMatrix<T> retSM(R); 1231 retSM = -retSM; 1232 retSM += L; // will zeroize(T(0)) 1233 1234 return retSM; 1235 } 1236 1237 //--------------------------------------------------------------------------- 1238 /// Matrix addition: SparseMatrix += SparseMatrix 1239 template <class T> operator +=(const SparseMatrix<T> & SM)1240 SparseMatrix<T>& SparseMatrix<T>::operator+=(const SparseMatrix<T>& SM) 1241 { 1242 if(SM.cols() != cols() || SM.rows() != rows()) 1243 GPSTK_THROW(Exception("Incompatible dimensions op+=(SM)")); 1244 //std::cout << "SM::op+=(SM)" << std::endl; 1245 1246 // loop over rows of SM 1247 typename std::map< unsigned int, SparseVector<T> >::const_iterator sit; 1248 for(sit = SM.rowsMap.begin(); sit != SM.rowsMap.end(); ++sit) { 1249 // find same row in this 1250 if(rowsMap.find(sit->first) == rowsMap.end()) 1251 rowsMap[sit->first] = sit->second; 1252 else 1253 rowsMap[sit->first] += sit->second; 1254 } 1255 1256 zeroize(T(0)); 1257 return *this; 1258 } 1259 1260 /// Matrix addition: SparseMatrix += Matrix 1261 template <class T> operator +=(const Matrix<T> & M)1262 SparseMatrix<T>& SparseMatrix<T>::operator+=(const Matrix<T>& M) 1263 { 1264 if(M.cols() != cols() || M.rows() != rows()) 1265 GPSTK_THROW(Exception("Incompatible dimensions op+=(M)")); 1266 //std::cout << "SM::op+=(M)" << std::endl; 1267 1268 // loop over rows of M 1269 for(unsigned int i=0; i<M.rows(); i++) { 1270 Vector<T> rowV(M.rowCopy(i)); 1271 if(rowsMap.find(i) == rowsMap.end()) { 1272 SparseVector<T> SV(rowV); 1273 rowsMap[i] = SV; 1274 } 1275 else 1276 rowsMap[i] += rowV; 1277 } 1278 1279 zeroize(T(0)); 1280 return *this; 1281 } 1282 1283 //--------------------------------------------------------------------------- 1284 /// Multiply all elements by a scalar T constant 1285 template <class T> operator *=(const T & value)1286 SparseMatrix<T>& SparseMatrix<T>::operator*=(const T& value) 1287 { 1288 if(value == T(0)) { 1289 rowsMap.clear(); 1290 return *this; 1291 } 1292 1293 // loop over all elements 1294 typename std::map< unsigned int, SparseVector<T> >::iterator it; 1295 typename std::map< unsigned int, T >::iterator vt; 1296 for(it = rowsMap.begin(); it != rowsMap.end(); ++it) { 1297 for(vt = it->second.vecMap.begin(); vt != it->second.vecMap.end(); ++vt) 1298 vt->second *= value; 1299 } 1300 1301 return *this; 1302 } 1303 1304 /// Divide all elements by a scalar T constant 1305 /// @throw Exception if the constant is zero 1306 template <class T> operator /=(const T & value)1307 SparseMatrix<T>& SparseMatrix<T>::operator/=(const T& value) 1308 { 1309 if(value == T(0)) GPSTK_THROW(Exception("Divide by zero")); 1310 1311 // loop over all elements 1312 typename std::map< unsigned int, SparseVector<T> >::iterator it; 1313 typename std::map< unsigned int, T >::iterator vt; 1314 for(it = rowsMap.begin(); it != rowsMap.end(); ++it) { 1315 for(vt = it->second.vecMap.begin(); vt != it->second.vecMap.end(); ++vt) 1316 vt->second /= value; 1317 } 1318 1319 return *this; 1320 } 1321 1322 /// Matrix addition: SparseMatrix = SparseMatrix + SparseMatrix : copy, += SM 1323 template <class T> operator +(const SparseMatrix<T> & L,const SparseMatrix<T> & R)1324 SparseMatrix<T> operator+(const SparseMatrix<T>& L, const SparseMatrix<T>& R) 1325 { 1326 if(L.cols() != R.cols() || L.rows() != R.rows()) 1327 GPSTK_THROW(Exception("Incompatible dimensions op+(SM,SM)")); 1328 //std::cout << "SM::op+(SM,SM)" << std::endl; 1329 1330 SparseMatrix<T> retSM(L); 1331 retSM -= R; // will zeroize(T(0)) 1332 1333 return retSM; 1334 } 1335 1336 /// Matrix addition: SparseMatrix = SparseMatrix + Matrix : copy, += M 1337 template <class T> operator +(const SparseMatrix<T> & L,const Matrix<T> & R)1338 SparseMatrix<T> operator+(const SparseMatrix<T>& L, const Matrix<T>& R) 1339 { 1340 if(L.cols() != R.cols() || L.rows() != R.rows()) 1341 GPSTK_THROW(Exception("Incompatible dimensions op+(SM,M)")); 1342 //std::cout << "SM::op+(SM,M)" << std::endl; 1343 1344 SparseMatrix<T> retSM(L); 1345 retSM -= R; // will zeroize(T(0)) 1346 1347 return retSM; 1348 } 1349 1350 /// Matrix addition: SparseMatrix = Matrix + SparseMatrix : copy, += M in rev order 1351 template <class T> operator +(const Matrix<T> & L,const SparseMatrix<T> & R)1352 SparseMatrix<T> operator+(const Matrix<T>& L, const SparseMatrix<T>& R) 1353 { 1354 if(L.cols() != R.cols() || L.rows() != R.rows()) 1355 GPSTK_THROW(Exception("Incompatible dimensions op+(M,SM)")); 1356 //std::cout << "SM::op+(M,SM)" << std::endl; 1357 1358 SparseMatrix<T> retSM(L); 1359 retSM -= R; // will zeroize(T(0)) 1360 1361 return retSM; 1362 } 1363 1364 //--------------------------------------------------------------------------- 1365 // row, col and diagonal copy, swap 1366 //--------------------------------------------------------------------------- 1367 /// return row i of this SparseMatrix as a SparseVector 1368 template <class T> rowCopy(const unsigned int i) const1369 SparseVector<T> SparseMatrix<T>::rowCopy(const unsigned int i) const 1370 { 1371 SparseVector<T> retSV; 1372 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 1373 it = rowsMap.find(i); 1374 if(it != rowsMap.end()) 1375 retSV = it->second; 1376 return retSV; 1377 } 1378 1379 /// return col j of this SparseMatrix as a SparseVector 1380 template <class T> colCopy(const unsigned int j) const1381 SparseVector<T> SparseMatrix<T>::colCopy(const unsigned int j) const 1382 { 1383 SparseVector<T> retSV; 1384 1385 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 1386 for(it = rowsMap.begin(); it != rowsMap.end(); ++it) { // loop over rows 1387 if(it->second.isFilled(j)) 1388 retSV.vecMap[it->first] = it->second[j]; 1389 } 1390 retSV.len = rows(); 1391 1392 return retSV; 1393 } 1394 1395 /// return diagonal of this SparseMatrix as a SparseVector 1396 template <class T> diagCopy(void) const1397 SparseVector<T> SparseMatrix<T>::diagCopy(void) const 1398 { 1399 SparseVector<T> retSV; 1400 1401 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 1402 for(it = rowsMap.begin(); it != rowsMap.end(); ++it) { // loop over rows 1403 if(it->second.isFilled(it->first)) 1404 retSV.vecMap[it->first] = it->second[it->first]; 1405 } 1406 retSV.len = rows(); 1407 1408 return retSV; 1409 } 1410 1411 /// swap rows of this SparseMatrix 1412 template <class T> swapRows(const unsigned int & ii,const unsigned int & jj)1413 void SparseMatrix<T>::swapRows(const unsigned int& ii, const unsigned int& jj) 1414 { 1415 if(ii >= nrows || jj >= nrows) GPSTK_THROW(Exception("Invalid indexes")); 1416 1417 typename std::map< unsigned int, SparseVector<T> >::iterator it, jt; 1418 it = rowsMap.find(ii); 1419 jt = rowsMap.find(jj); 1420 if(it == rowsMap.end()) { 1421 if(jt == rowsMap.end()) return; // nothing to do 1422 else { 1423 rowsMap[ii] = rowsMap[jj]; 1424 rowsMap.erase(jt); 1425 } 1426 } 1427 else { 1428 if(jt == rowsMap.end()) { 1429 rowsMap[jj] = rowsMap[ii]; 1430 rowsMap.erase(it); 1431 } 1432 else { 1433 SparseVector<T> save(it->second); 1434 rowsMap[ii] = rowsMap[jj]; 1435 rowsMap[jj] = save; 1436 } 1437 } 1438 } 1439 1440 /// swap columns of this SparseMatrix 1441 template <class T> swapCols(const unsigned int ii,const unsigned int jj)1442 void SparseMatrix<T>::swapCols(const unsigned int ii, const unsigned int jj) 1443 { 1444 if(ii >= ncols || jj >= ncols) GPSTK_THROW(Exception("Invalid indexes")); 1445 1446 // may not be the fastest, but may be fast enough - tranpose() is fast 1447 SparseMatrix<T> trans(transpose(*this)); 1448 trans.swapRows(ii,jj); 1449 *this = transpose(*this); 1450 } 1451 1452 //--------------------------------------------------------------------------------- 1453 // special matrices 1454 //--------------------------------------------------------------------------------- 1455 /// Compute the identity matrix of dimension dim x dim 1456 /// @param dim dimension of desired identity matrix (dim x dim) 1457 /// @return identity matrix 1458 template <class T> identSparse(const unsigned int dim)1459 SparseMatrix<T> identSparse(const unsigned int dim) throw() 1460 { 1461 if(dim == 0) return SparseMatrix<T>(); 1462 1463 SparseMatrix<T> toRet(dim,dim); 1464 for(unsigned int i=0; i<dim; i++) { 1465 SparseVector<T> SV(dim); 1466 SV.vecMap[i] = T(1); 1467 toRet.rowsMap[i] = SV; 1468 } 1469 1470 return toRet; 1471 } 1472 1473 //--------------------------------------------------------------------------------- 1474 // matrix products and transformations 1475 //--------------------------------------------------------------------------------- 1476 1477 //--------------------------------------------------------------------------------- 1478 // SM * transpose(SM) 1479 // NB this is barely faster than just forming SM * transpose(SM) 1480 template <class T> matrixTimesTranspose(const SparseMatrix<T> & SM)1481 SparseMatrix<T> matrixTimesTranspose(const SparseMatrix<T>& SM) 1482 { 1483 try { 1484 SparseMatrix<T> toRet(SM.rows(),SM.rows()); 1485 1486 typename std::map< unsigned int, SparseVector<T> >::const_iterator it, jt; 1487 for(it = SM.rowsMap.begin(); it != SM.rowsMap.end(); ++it) { 1488 SparseVector<T> Vrow(SM.rows()); 1489 toRet.rowsMap[it->first] = Vrow; 1490 for(jt = SM.rowsMap.begin(); jt != SM.rowsMap.end(); ++jt) { 1491 T d(dot(it->second,jt->second)); 1492 if(d != T(0)) toRet.rowsMap[it->first][jt->first] = d; 1493 } 1494 } 1495 1496 return toRet; 1497 1498 } catch(Exception& e) { GPSTK_RETHROW(e); } 1499 } 1500 1501 //--------------------------------------------------------------------------------- 1502 //--------------------------------------------------------------------------------- 1503 /// Compute diagonal of P*C*P^T, ie diagonal of transform of square Matrix C. 1504 template <class T> transformDiag(const SparseMatrix<T> & P,const Matrix<T> & C)1505 Vector<T> transformDiag(const SparseMatrix<T>& P, const Matrix<T>& C) 1506 { 1507 try { 1508 if(P.cols() != C.rows() || C.rows() != C.cols()) 1509 GPSTK_THROW(Exception("Incompatible dimensions transformDiag()")); 1510 1511 const unsigned int n(P.cols()); 1512 typename std::map< unsigned int, SparseVector<T> >::const_iterator jt; 1513 typename std::map< unsigned int, T >::const_iterator vt; 1514 1515 Vector<T> toRet(P.rows(),T(0)); 1516 T sum; 1517 1518 // loop over rows of P = columns of transpose(P) 1519 for(jt = P.rowsMap.begin(); jt != P.rowsMap.end(); ++jt) { 1520 unsigned int j = jt->first; // row of P, col of P^T 1521 Vector<T> prod(n,T(0)); 1522 1523 // loop over columns of C up to and including j, 1524 // forming dot product prod(k) = P(rowj) * C(colk) 1525 for(unsigned int k=0; k<n; k++) { 1526 sum = T(0); 1527 for(vt=jt->second.vecMap.begin(); vt!=jt->second.vecMap.end(); ++vt) 1528 sum += vt->second * C(vt->first,k); 1529 if(sum != T(0)) prod(k) = sum; 1530 } 1531 toRet(j) = dot(jt->second,prod); 1532 } 1533 return toRet; 1534 1535 } catch(Exception& e) { GPSTK_RETHROW(e); } 1536 } 1537 1538 //--------------------------------------------------------------------------- 1539 // inverse (via Gauss-Jordan) 1540 //--------------------------------------------------------------------------- 1541 /// inverse via Gauss-Jordan; NB GJ involves only row operations. 1542 /// NB not the best numerically; for high condition number, use inverseViaCholesky, 1543 /// or cast to Matrix, use either LUD or SVD, then cast back to SparseMatrix. 1544 template <class T> inverse(const SparseMatrix<T> & A)1545 SparseMatrix<T> inverse(const SparseMatrix<T>& A) 1546 { 1547 try { 1548 if(A.rows() != A.cols() || A.rows() == 0) { 1549 std::ostringstream oss; 1550 oss << "Invalid input dimensions: " << A.rows() << "x" << A.cols(); 1551 GPSTK_THROW(Exception(oss.str())); 1552 } 1553 1554 unsigned int i,k; 1555 T dtmp; 1556 //T big, small; 1557 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 1558 1559 // does A have any zero rows? 1560 for(it = A.rowsMap.begin(), i=0; it != A.rowsMap.end(); i++, ++it) { 1561 if(i != it->first) { 1562 std::ostringstream oss; 1563 oss << "Singular matrix - zero row at index " << i << ")"; 1564 GPSTK_THROW(Exception(oss.str())); 1565 } 1566 } 1567 1568 const unsigned int N(A.rows()); 1569 typename std::map< unsigned int, SparseVector<T> >::iterator jt,kt; 1570 typename std::map< unsigned int, T >::iterator vt; 1571 SparseMatrix<T> GJ(A || identSparse<T>(N)); 1572 1573 //std::cout << "\nInitial:\n" << std::scientific 1574 // << std::setprecision(2) << std::setw(10) << GJ << std::endl; 1575 1576 // loop over rows of work matrix, making lower left triangular = unity 1577 for(jt = GJ.rowsMap.begin(); jt != GJ.rowsMap.end(); ++jt) { 1578 1579 // divide row by diagonal element; if diagonal is zero, add another row 1580 vt = jt->second.vecMap.find(jt->first); // diagonal element GJ(j,j) 1581 if(vt == jt->second.vecMap.end() || vt->second == T(0)) { 1582 // find a lower row with non-zero element (same col); add to this row 1583 for((kt=jt)++; kt != GJ.rowsMap.end(); ++kt) { 1584 vt = kt->second.vecMap.find(jt->first); // GJ(k,j) 1585 if(vt == kt->second.vecMap.end() || vt->second == T(0)) 1586 continue; // nope, its zero 1587 1588 // add the kt row to the jt row 1589 jt->second += kt->second; 1590 break; 1591 } 1592 if(kt == GJ.rowsMap.end()) 1593 GPSTK_THROW(Exception("Singular matrix")); 1594 } 1595 1596 dtmp = vt->second; 1597 // are these scales 1/dtmp related to condition number? eigenvalues? det? 1598 // they are close to condition number.... 1599 //if(jt == GJ.rowsMap.begin()) big = small = ::fabs(dtmp); 1600 //if(::fabs(dtmp) > big) big = ::fabs(dtmp); 1601 //if(::fabs(dtmp) < small) small = ::fabs(dtmp); 1602 1603 // normalize the j row 1604 if(dtmp != T(1)) jt->second *= T(1)/dtmp; 1605 1606 //std::cout << "\nRow " << jt->first << " scaled with " << std::scientific 1607 // << std::setprecision(2) << T(1)/dtmp << "\n" 1608 // << std::setw(10) << GJ << std::endl; 1609 1610 // now zero out the column below the j diagonal 1611 for((kt=jt)++; kt != GJ.rowsMap.end(); ++kt) { 1612 vt = kt->second.vecMap.find(jt->first); // GJ(k,j) 1613 if(vt == kt->second.vecMap.end() || vt->second == T(0)) 1614 continue; // already zero 1615 1616 kt->second.addScaledSparseVector(-vt->second, jt->second); 1617 } 1618 1619 //std::cout << "\nRow " << jt->first << " left-zeroed:\n" 1620 // << std::scientific << std::setprecision(2) << std::setw(10) 1621 // << GJ << std::endl; 1622 } 1623 1624 // loop over rows of work matrix in reverse order, 1625 // zero-ing out the column above the diag 1626 typename std::map< unsigned int, SparseVector<T> >::reverse_iterator rjt,rkt; 1627 for(rjt = GJ.rowsMap.rbegin(); rjt != GJ.rowsMap.rend(); ++rjt) { 1628 // now zero out the column above the j diagonal 1629 for((rkt=rjt)++; rkt != GJ.rowsMap.rend(); ++rkt) { 1630 vt = rkt->second.vecMap.find(rjt->first); // GJ(k,j) 1631 if(vt == rkt->second.vecMap.end() || vt->second == T(0)) 1632 continue; // already zero 1633 rkt->second.addScaledSparseVector(-vt->second, rjt->second); 1634 } 1635 1636 //std::cout << "\nRow " << rjt->first << " right-zeroed:\n" 1637 // << std::scientific << std::setprecision(2) << std::setw(10) 1638 // << GJ << std::endl; 1639 } 1640 1641 //std::cout << "\nbig and small for this matrix are: " 1642 // << std::scientific << std::setprecision(2) 1643 // << big << " " << small << " with ratio " << big/small << std::endl; 1644 1645 return (SparseMatrix<T>(GJ,0,N,N,N)); 1646 1647 } catch(Exception& e) { GPSTK_RETHROW(e); } 1648 } 1649 1650 //--------------------------------------------------------------------------- 1651 // Factorization, decomposition and other algorithms 1652 //--------------------------------------------------------------------------- 1653 1654 //--------------------------------------------------------------------------------- 1655 // Compute Cholesky decomposition of symmetric positive definite matrix using Crout 1656 // algorithm. A = L*L^T where A and L are (nxn) and L is lower triangular reads: 1657 // [ A00 A01 A02 ... A0n ] = [ L00 0 0 0 ... 0 ][ L00 L10 L20 ... L0n ] 1658 // [ A10 A11 A12 ... A1n ] = [ L10 L11 0 0 ... 0 ][ 0 L11 L21 ... L1n ] 1659 // [ A20 A21 A22 ... A2n ] = [ L20 L21 L22 0 ... 0 ][ 0 0 L22 ... L2n ] 1660 // ... ... ... 1661 // [ An0 An1 An2 ... Ann ] = [ Ln0 Ln1 Ln2 0 ... Lnn][ 0 0 0 ... Lnn ] 1662 // but multiplying out gives 1663 // A = [ L00^2 1664 // [ L00*L10 L11^2+L10^2 1665 // [ L00*L20 L11*L21+L10*L20 L22^2+L21^2+L20^2 1666 // ... 1667 // Aii = Lii^2 + sum(k=0,i-1) Lik^2 1668 // Aij = Lij*Ljj + sum(k=0,j-1) Lik*Ljk 1669 // These can be inverted by looping over columns, and filling L from diagonal down. 1670 // So fill L in this way 1671 // d do diagonal element first, then the column below it 1672 // 1d at each row i below the diagonal, save the element^2 in rowSums[i] 1673 // 12d 1674 // 123d 1675 // 123 d 1676 // 123 d 1677 // 123 d 1678 // 123 d 1679 // 123 etc d 1680 1681 /// Compute lower triangular square root of a symmetric positive definite matrix 1682 /// (Cholesky decomposition) Crout algorithm. 1683 /// @param A SparseMatrix to be decomposed; symmetric and positive definite, const 1684 /// @return SparseMatrix lower triangular square root of input matrix 1685 /// @throw if input SparseMatrix is not square 1686 /// @throw if input SparseMatrix is not positive definite 1687 template <class T> lowerCholesky(const SparseMatrix<T> & A)1688 SparseMatrix<T> lowerCholesky(const SparseMatrix<T>& A) 1689 { 1690 if(A.rows() != A.cols() || A.rows() == 0) { 1691 std::ostringstream oss; 1692 oss << "Invalid input dimensions: " << A.rows() << "x" << A.cols(); 1693 GPSTK_THROW(Exception(oss.str())); 1694 } 1695 1696 const unsigned int n=A.rows(); 1697 unsigned int i,j,k; 1698 T d, diag; 1699 SparseMatrix<T> L(n,n); // compute the answer 1700 std::vector<T> rowSums; // keep sum(k=0..j-1)[L(j,k)^2] for each row j 1701 typename std::map< unsigned int, SparseVector<T> >::const_iterator it, jt; 1702 typename std::map< unsigned int, SparseVector<T> >::iterator Lit, Ljt; 1703 1704 // A must have all rows - a zero row in A means its singular 1705 // create all the rows in L; all exist b/c if any diagonal is 0 -> singular 1706 // fill rowSums vector with zeros 1707 for(it=A.rowsMap.begin(), i=0; it!=A.rowsMap.end(); i++, ++it) { 1708 if(i != it->first) { 1709 std::ostringstream oss; 1710 oss << "lowerCholesky() requires positive-definite input:" 1711 << " (zero rows at index " << i << ")"; 1712 GPSTK_THROW(Exception(oss.str())); 1713 } 1714 1715 SparseVector<T> Vrow(n); 1716 L.rowsMap[i] = Vrow; 1717 1718 rowSums.push_back(T(0)); 1719 } 1720 1721 // loop over columns of A, at the same time looping over rows of L 1722 // use jt to iterate over the columns of A, keeping count with (column) j 1723 for(jt = A.rowsMap.begin(), Ljt = L.rowsMap.begin(); 1724 jt != A.rowsMap.end() && Ljt != L.rowsMap.end(); ++jt, ++Ljt) 1725 { 1726 j = jt->first; // column j (A) or row i (L) 1727 1728 // compute the j,j diagonal element of L 1729 // start with diagonal of A(j,j) 1730 d = jt->second[j]; // A(j,j) 1731 1732 // subtract sum(k=0..j-1)[L(j,k)^2] 1733 d -= rowSums[j]; 1734 1735 // d is the eigenvalue - must not be zero 1736 if(d <= T(0)) { 1737 std::ostringstream oss; 1738 oss << "Non-positive eigenvalue " << std::scientific << d << " at col " 1739 << j << ": lowerCholesky() requires positive-definite input"; 1740 GPSTK_THROW(Exception(oss.str())); 1741 } 1742 1743 diag = SQRT(d); 1744 L.rowsMap[j].vecMap[j] = diag; // L(j,j) 1745 1746 // now loop over rows below the diagonal, filling in this column 1747 Lit = Ljt; 1748 it = jt; 1749 for(++Lit, ++it; Lit != L.rowsMap.end(); ++Lit, ++it) { 1750 i = Lit->first; 1751 d = (it->second.isFilled(j) ? it->second[j] : T(0)); 1752 d -= dot_lim(Lit->second, Ljt->second, 0, j); 1753 1754 if(d != T(0)) { 1755 d /= diag; 1756 Lit->second.vecMap[j] = d; 1757 rowSums[i] += d*d; // save L(i,j)^2 term 1758 } 1759 1760 } // end loop over rows below the diagonal 1761 1762 } // end loop over column j of A 1763 1764 return L; 1765 } 1766 1767 //--------------------------------------------------------------------------------- 1768 /// Compute inverse of lower-triangular SparseMatrix 1769 template <class T> inverseLT(const SparseMatrix<T> & L,T * ptrSmall,T * ptrBig)1770 SparseMatrix<T> inverseLT(const SparseMatrix<T>& L, T *ptrSmall, T *ptrBig) 1771 { 1772 if(L.rows() != L.cols() || L.rows() == 0) { 1773 std::ostringstream oss; 1774 oss << "Invalid input dimensions: " << L.rows() << "x" << L.cols(); 1775 GPSTK_THROW(Exception(oss.str())); 1776 } 1777 1778 const unsigned int n(L.rows()); 1779 unsigned int i,j,k; 1780 T big(0), small(0), dum, sum; 1781 1782 // trick is to fill transpose(inverse) and then transpose at the end 1783 SparseMatrix<T> invLT(L.cols(),L.rows()); 1784 typename std::map< unsigned int, SparseVector<T> >::const_iterator it; 1785 typename std::map< unsigned int, SparseVector<T> >::iterator jt; 1786 1787 // do the diagonal first; this finds singularities and defines all rows in InvLT 1788 for(i=0, it = L.rowsMap.begin(); i < n; ++it, ++i) { 1789 if(it == L.rowsMap.end() || it->first != i || 1790 !it->second.isFilled(i) || it->second[i]==T(0)) 1791 { 1792 std::ostringstream oss; 1793 oss << "Singular matrix - zero diagonal at row " << i; 1794 GPSTK_THROW(Exception(oss.str())); 1795 } 1796 1797 dum = it->second[i]; 1798 if(ptrSmall) { 1799 if(ABS(dum) > big) big = ABS(dum); 1800 if(ABS(dum) < small) small = ABS(dum); 1801 } 1802 1803 // create row i and element i,i in the answer 1804 dum = T(1)/dum; 1805 SparseVector<T> SV(L.cols()); 1806 SV.vecMap[i] = dum; 1807 invLT.rowsMap[i] = SV; 1808 } 1809 1810 // loop over rows again, filling in below the diagonal 1811 // (L has all rows present, else its singular above) 1812 //for(i=1; i<n; i++ 1813 it = L.rowsMap.begin(); 1814 for(++it; it != L.rowsMap.end(); ++it) { 1815 dum = T(1)/it->second[it->first]; // has to be there, and non-zero 1816 // loop over columns of invL (rows of invLT) before the diagonal 1817 // store results temporarily in a map 1818 std::map<unsigned int,T> tempMap; 1819 //for(j=0; j<i; j++) 1820 for(jt = invLT.rowsMap.begin(); jt != invLT.rowsMap.end(); ++jt) { 1821 if(jt->first >= it->first) break; 1822 //sum=0; for(k=j;k<i;k++) sum += L(i,k)*invLT(j,k) 1823 sum = dot_lim(it->second, jt->second, jt->first, it->first); 1824 //invLT(j,i) = -sum*dum 1825 if(sum != T(0)) tempMap[jt->first] = -dum*sum; 1826 //jt->second.vecMap[it->first] = -dum*sum; 1827 } 1828 // now move contents of tempMap to invLT 1829 typename std::map<unsigned int,T>::iterator tt = tempMap.begin(); 1830 for( ; tt != tempMap.end(); ++tt) 1831 invLT.rowsMap[tt->first].vecMap[it->first] = tt->second; // invLT(j,i) 1832 } 1833 1834 if(ptrSmall) *ptrSmall = small; 1835 if(ptrBig) *ptrBig = big; 1836 1837 return transpose(invLT); 1838 } 1839 1840 //--------------------------------------------------------------------------------- 1841 /// Compute upper triangular square root of a symmetric positive definite matrix 1842 /// (Cholesky decomposition) Crout algorithm; that is A = transpose(U)*U. 1843 /// Note that this result will be equal to 1844 /// transpose(lowerCholesky(A)) == transpose(Ch.L from class Cholesky), NOT Ch.U; 1845 /// class Cholesky computes L,U where A = L*LT = U*UT [while A=UT*U here]. 1846 /// @param A SparseMatrix to be decomposed; symmetric and positive definite, const 1847 /// @return SparseMatrix upper triangular square root of input matrix 1848 /// @throw Exception if input SparseMatrix is not square 1849 /// @throw Exception if input SparseMatrix is not positive definite 1850 template <class T> upperCholesky(const SparseMatrix<T> & A)1851 SparseMatrix<T> upperCholesky(const SparseMatrix<T>& A) 1852 { return transpose(lowerCholesky(A)); } 1853 1854 //--------------------------------------------------------------------------------- 1855 /// Compute inverse of a symmetric positive definite matrix using Cholesky 1856 /// decomposition. 1857 /// @param A SparseMatrix to be inverted; symmetric and positive definite, const 1858 /// @return SparseMatrix inverse of input matrix 1859 /// @throw Exception if input SparseMatrix is not square, not positive definite, or singular 1860 template <class T> inverseViaCholesky(const SparseMatrix<T> & A)1861 SparseMatrix<T> inverseViaCholesky(const SparseMatrix<T>& A) 1862 { 1863 try { 1864 //SparseMatrix<T> L(lowerCholesky(A)); 1865 //SparseMatrix<T> Linv(inverseLT(L)); 1866 //SparseMatrix<T> Ainv(matrixTimesTranspose(transpose(Linv))); 1867 //return Ainv; 1868 return (matrixTimesTranspose(transpose(inverseLT(lowerCholesky(A))))); 1869 } 1870 catch(Exception& me) { 1871 me.addText("Called by inverseViaCholesky()"); 1872 GPSTK_RETHROW(me); 1873 } 1874 } 1875 1876 //--------------------------------------------------------------------------------- 1877 /// Householder transformation of a matrix. 1878 template <class T> SparseHouseholder(const SparseMatrix<T> & A)1879 SparseMatrix<T> SparseHouseholder(const SparseMatrix<T>& A) 1880 { 1881 unsigned int i,j,k; 1882 typename std::map< unsigned int, SparseVector<T> >::iterator jt,kt,it; 1883 typename std::map< unsigned int, T >::iterator vt; 1884 1885 SparseMatrix<T> AT(transpose(A)); // perform the algorithm on the transpose 1886 1887 // loop over rows (columns of input A) 1888 for(j=0; (j<A.cols()-1 && j<A.rows()-1); j++) { 1889 jt = AT.rowsMap.find(j); // is column j there? 1890 if(jt == AT.rowsMap.end()) // no, so A is already zero below diag 1891 continue; 1892 1893 // pull out column j (use only below and including diagonal, ignore V(i<j)) 1894 SparseVector<T> V(jt->second); 1895 T sum(0); 1896 for(vt=V.vecMap.begin(); vt!=V.vecMap.end(); ++vt) { 1897 if(vt->first < j) continue; 1898 sum += vt->second * vt->second; 1899 } 1900 if(sum < T(1.e-20)) continue; // col j is already zero below diag 1901 1902 //zero out below diagonal - must remove element 1903 vt = jt->second.vecMap.lower_bound(jt->first); 1904 if(vt != jt->second.vecMap.end()) 1905 jt->second.vecMap.erase(vt,jt->second.vecMap.end()); 1906 1907 sum = SQRT(sum); 1908 vt = V.vecMap.find(j); 1909 if(vt != V.vecMap.end()) { 1910 if(vt->second > T(0)) sum = -sum; 1911 jt->second[j] = sum; // A(j,j) = sum 1912 vt->second -= sum; // V(j) -= sum 1913 sum = T(1)/(sum*vt->second); 1914 } 1915 else { 1916 jt->second[j] = sum; // A(j,j) = sum 1917 V.vecMap[j] = -sum; // V(j) -= sum 1918 sum = T(-1)/(sum*sum); 1919 } 1920 1921 // loop over columns beyond j 1922 kt = jt; 1923 for(++kt; kt != AT.rowsMap.end(); ++kt) { 1924 T alpha(0); 1925 for(vt=kt->second.vecMap.begin(); vt!=kt->second.vecMap.end(); ++vt) { 1926 if(vt->first < j) continue; 1927 i = vt->first; 1928 if(V.isFilled(i)) // alpha += A(i,k)*V(i) 1929 alpha += vt->second * V.vecMap[i]; 1930 } 1931 alpha *= sum; 1932 if(alpha == T(0)) continue; 1933 // modify column k at and below j 1934 for(i=jt->first; i<AT.cols(); i++) { 1935 if(!V.isFilled(i)) continue; 1936 vt = kt->second.vecMap.find(i); 1937 if(vt == kt->second.vecMap.end()) { // create element 1938 kt->second.vecMap[i] = alpha * V.vecMap[i]; 1939 } 1940 else { 1941 kt->second.vecMap[i] += alpha * V.vecMap[i]; 1942 } 1943 } 1944 } 1945 } 1946 1947 return (transpose(AT)); 1948 } 1949 1950 //--------------------------------------------------------------------------------- 1951 // This routine uses the Householder algorithm to update the SRI state+covariance. 1952 // Input: 1953 // R a priori SRI matrix (upper triangular, dimension N) 1954 // Z a priori SRI data vector (length N) 1955 // A concatentation of H and D : A = H || D, where 1956 // H Measurement partials, an M by N matrix. 1957 // D Data vector, of length M 1958 // Output: Updated R and Z. H is trashed, but the data vector D 1959 // contains the residuals of fit (D - A*state). 1960 // Return values: SrifMU returns void, but throws exceptions if the input matrices 1961 // or vectors have incompatible dimensions. 1962 // 1963 // Measurment noise associated with H and D must be white with unit covariance. 1964 // If necessary, the data can be 'whitened' before calling this routine in order 1965 // to satisfy this requirement. This is done as follows. 1966 // Compute the lower triangular square root of the covariance matrix, L, 1967 // and replace H with inverse(L)*H and D with inverse(L)*D. 1968 // 1969 // The Householder transformation is simply an orthogonal transformation 1970 // designed to make the elements below the diagonal zero. It works by explicitly 1971 // performing the transformation, one column at a time, without actually 1972 // constructing the transformation matrix. The matrix is transformed as follows 1973 // [ A(m,n) ] => [ sum a ] 1974 // [ ] => [ 0 A'(m-1,n-1) ] 1975 // after which the same transformation is applied to A' matrix, until A' has only 1976 // one row or column. The transformation that zeros the diagonal below the (k,k) 1977 // element also replaces the (k,k) element and modifies the matrix elements for 1978 // columns >= k and rows >=k, but does not affect the matrix for columns < k 1979 // or rows < k. 1980 // Column k (=0..min(m,n)-1) of the input matrix A(m,n) can be zeroed 1981 // below the diagonal (columns < k have already been so zeroed) as follows: 1982 // let y be the vector equal to column k at the diagonal and below, 1983 // ( so y(j)==A(k+j,k), y(0)==A(k,k), y.size = m-k ) 1984 // let sum = -sign(y(0))*|y|, 1985 // define vector u by u(0) = y(0)-sum, u(j)=y(j) for j>0 (j=1..m-k) 1986 // and finally define b = 1/(sum*u(0)). 1987 // Redefine column k with A(k,k)=sum and A(k+j,k)=0, j=1..m, and then for 1988 // each column j > k, (j=k+1..n) 1989 // compute g = b*sum[u(i)*A(k+i,j)], i=0..m-k-1, 1990 // replace A(k+i,j) with A(k+i,j)+g*u(i), for i=0..m-k-1 1991 // Most algorithms don't handle special cases: 1992 // 1. If column k is already zero below the diagonal, but A(k,k)!=0, then 1993 // y=[A(k,k),0,0,...0], sum=-A(k,k), u(0)=2A(k,k), u=[2A(k,k),0,0,...0] 1994 // and b = -1/(2*A(k,k)^2). Then, zeroing column k only changes the sign 1995 // of A(k,k), and for the other columns j>k, g = -A(k,j)/A(k,k) and only 1996 // row k is changed. 1997 // 2. If column k is already zero below the diagonal, AND A(k,k) is zero, 1998 // then y=0,sum=0,u=0 and b is infinite...the transformation is undefined. 1999 // However this column should be skipped (Biermann Appendix VII.B). 2000 // 2001 // Ref: Bierman, G.J. "Factorization Methods for Discrete Sequential 2002 // Estimation," Academic Press, 1977. 2003 // 2004 /// Square root information measurement update, with new data in the form of a 2005 /// single SparseMatrix concatenation of H and D: A = H || D. 2006 /// See doc for the overloaded SrifMU(). 2007 template <class T> SrifMU(Matrix<T> & R,Vector<T> & Z,SparseMatrix<T> & A,const unsigned int M)2008 void SrifMU(Matrix<T>& R, Vector<T>& Z, SparseMatrix<T>& A, const unsigned int M) 2009 { 2010 // if necessary, create R and Z 2011 if(A.cols() > 1 && R.rows() == 0 && Z.size() == 0) { 2012 R = Matrix<double>(A.cols()-1,A.cols()-1,0.0); 2013 Z = Vector<double>(A.cols()-1,0.0); 2014 } 2015 2016 if(A.cols() <= 1 || A.cols() != R.cols()+1 || Z.size() < R.rows()) { 2017 std::ostringstream oss; 2018 oss << "Invalid input dimensions:\n R has dimension " 2019 << R.rows() << "x" << R.cols() << ",\n Z has length " 2020 << Z.size() << ",\n and A has dimension " 2021 << A.rows() << "x" << A.cols(); 2022 GPSTK_THROW(Exception(oss.str())); 2023 } 2024 2025 const T EPS=T(1.e-20); 2026 const unsigned int m(M==0 || M>A.rows() ? A.rows() : M), n(R.rows()); 2027 const unsigned int np1(n+1); // if np1 = n, state vector Z is not updated 2028 unsigned int i,j,k; 2029 T dum, delta, beta; 2030 typename std::map< unsigned int, SparseVector<T> >::iterator jt,kt,it; 2031 typename std::map< unsigned int, T >::iterator vt; 2032 2033 SparseMatrix<T> AT(transpose(A)); // work with the transpose 2034 2035 for(j=0; j<n; j++) { // loop over columns 2036 jt = AT.rowsMap.find(j); // is column j empty? 2037 if(jt == AT.rowsMap.end()) // no, so A is already zero below the diagonal 2038 continue; 2039 2040 // pull out column j of A; it is entirely below the diagonal 2041 SparseVector<T> Vj(jt->second); 2042 T sum(dot(Vj,Vj)); 2043 //T sum(0); 2044 //for(i=0; i<m; i++) 2045 // sum += A(i,j)*A(i,j); // sum of squares of elements in column below diag 2046 if(sum < EPS) continue; // sum is positive 2047 2048 dum = R(j,j); 2049 sum += dum * dum; // add diagonal element 2050 sum = (dum > T(0) ? -T(1) : T(1)) * SQRT(sum); 2051 delta = dum - sum; 2052 R(j,j) = sum; 2053 2054 //if(j+1 > np1) break; // this in case np1 is ever set to n .... 2055 2056 beta = sum*delta; // beta by construction must be negative 2057 if(beta > -EPS) continue; 2058 beta = T(1)/beta; 2059 2060 kt = jt; 2061 for(k=j+1; k<np1; k++) { // columns to right of diagonal (j,j) 2062 SparseVector<T> Vk(m); // will be column k of A 2063 2064 if(kt->first == k-1) ++kt; // kt now points to next column -- is it k? 2065 if(kt->first != k) { // no col k - should create a column in A... 2066 AT.rowsMap[k] = Vk; 2067 kt = AT.rowsMap.find(k); 2068 } 2069 else 2070 Vk = kt->second; // now Vk is column k of A, perhaps empty 2071 2072 sum = delta * (k==n ? Z(j) : R(j,k)); 2073 sum += dot(Vk,Vj); 2074 // for(i=0; i<m; i++) 2075 // sum += A(i,j) * A(i,k); 2076 if(sum == T(0)) continue; 2077 2078 sum *= beta; 2079 if(k==n) Z(j) += sum*delta; 2080 else R(j,k) += sum*delta; 2081 2082 vt = kt->second.vecMap.begin(); 2083 for(i=0; i<m; i++) { // loop over rows in column k 2084 // A(i,k) += sum * A(i,j); 2085 if(vt != kt->second.vecMap.end() && vt->first == i) { 2086 vt->second += sum * Vj.vecMap[i]; 2087 ++vt; 2088 } 2089 else 2090 kt->second.vecMap[i] = sum * Vj.vecMap[i]; 2091 } 2092 } 2093 } 2094 2095 // must put last row of AT (last column of A) back into A - these are residuals 2096 jt = AT.rowsMap.find(AT.rows()-1); 2097 // should never happen 2098 if(jt == AT.rowsMap.end()) GPSTK_THROW(Exception("Failure on last column")); 2099 2100 // put this row, jt->second, into the last column of A 2101 j = A.cols()-1; 2102 i = 0; 2103 it = A.rowsMap.begin(); 2104 vt = jt->second.vecMap.begin(); 2105 while(it != A.rowsMap.end() && vt != jt->second.vecMap.end()) { 2106 if(it->first > vt->first) { // A has no row at index vt->first 2107 SparseVector<T> SV(A.cols()); 2108 SV.vecMap[j] = vt->second; 2109 A.rowsMap[vt->first] = SV; 2110 ++vt; 2111 } 2112 else if(vt->first > it->first) { // resids are missing at this row 2113 ++it; 2114 } 2115 else { // match - equal indexes 2116 A.rowsMap[vt->first].vecMap[j] = vt->second; 2117 ++it; 2118 ++vt; 2119 } 2120 } 2121 2122 } // end SrifMU 2123 2124 //--------------------------------------------------------------------------- 2125 template <class T> SrifMU(Matrix<T> & R,Vector<T> & Z,SparseMatrix<T> & P,Vector<T> & D,const unsigned int M)2126 void SrifMU(Matrix<T>& R, Vector<T>& Z, SparseMatrix<T>& P, 2127 Vector<T>& D, const unsigned int M) 2128 { 2129 try { 2130 SparseMatrix<T> A(P||D); 2131 SrifMU(R,Z,A,M); 2132 // copy residuals out of A into D 2133 D = Vector<T>(A.colCopy(A.cols()-1)); 2134 } 2135 catch(Exception& e) { GPSTK_RETHROW(e); } 2136 } 2137 2138 //--------------------------------------------------------------------------- 2139 //--------------------------------------------------------------------------- 2140 2141 } // namespace 2142 2143 #endif // define SPARSE_MATRIX_INCLUDE 2144