SparseMatrix.cpp
Go to the documentation of this file.
00001 
00028 #include <string>
00029 #include <cstring> // memcpy()
00030 #include <fstream>
00031 #include <iostream>
00032 #include <cmath>
00033 
00034 #include "isam/util.h"
00035 
00036 #include "isam/SparseMatrix.h"
00037 
00038 using namespace std;
00039 using namespace Eigen;
00040 
00041 namespace isam {
00042 
00043 // initial values don't matter much
00044 const int MIN_NUM_COLS = 10;
00045 const int MIN_NUM_ROWS = 10;
00046 
00047 void SparseMatrix::_allocate_SparseMatrix(int num_rows, int num_cols, int max_num_rows, int max_num_cols, bool init_rows) {
00048   _num_rows = num_rows;
00049   _num_cols = num_cols;
00050   _max_num_rows = max_num_rows;
00051   _max_num_cols = max_num_cols;
00052   _rows = new SparseVector_p[_max_num_rows];
00053   if (init_rows) {
00054     for (int row=0; row<_num_rows; row++) {
00055       _rows[row] = new SparseVector();
00056     }
00057   }
00058 }
00059 
00060 void SparseMatrix::_copy_from_SparseMatrix(const SparseMatrix& mat) {
00061   _allocate_SparseMatrix(mat._num_rows, mat._num_cols, mat._num_rows, mat._num_cols, false);
00062   for (int row=0; row<_num_rows; row++) {
00063     _rows[row] = new SparseVector(*mat._rows[row]);
00064   }
00065 }
00066 
00067 void SparseMatrix::_dealloc_SparseMatrix() {
00068   for (int row=0; row<_num_rows; row++) {
00069     delete _rows[row];
00070     _rows[row] = NULL;
00071   }
00072   delete[] _rows;
00073   _rows = NULL;
00074 }
00075 
00076 SparseMatrix::SparseMatrix(int num_rows, int num_cols) {
00077   requireDebug(num_rows>=0 && num_cols>=0, "SparseMatrix constructor: Index out of range");
00078   // we reserve space for at least 2 times what we need now, but not to be less than MIN_COLS
00079   _allocate_SparseMatrix(num_rows, num_cols, max(MIN_NUM_ROWS, 2*num_rows), max(MIN_NUM_COLS, 2*num_cols));
00080 }
00081 
00082 SparseMatrix::SparseMatrix(const SparseMatrix& mat) {
00083   _copy_from_SparseMatrix(mat);
00084 }
00085 
00086 SparseMatrix::SparseMatrix(const SparseMatrix& mat, int num_rows, int num_cols, int first_row, int first_col) {
00087   _allocate_SparseMatrix(num_rows, num_cols, num_rows, num_cols);
00088   for (int row=0; row<num_rows; row++) {
00089     // copy parts of rowvectors
00090     *_rows[row] = SparseVector(*mat._rows[row+first_row], num_rows, first_row);
00091   }
00092 }
00093 
00094 SparseMatrix::SparseMatrix(int num_rows, int num_cols, SparseVector_p* rows) {
00095   _num_rows = num_rows;
00096   _num_cols = num_cols;
00097   _max_num_rows = num_rows;
00098   _max_num_cols = num_cols;
00099   _rows = new SparseVector_p[_max_num_rows];
00100   for (int row=0; row<_num_rows; row++) {
00101     _rows[row] = rows[row];
00102   }
00103 }
00104 
00105 SparseMatrix::~SparseMatrix() {
00106   _dealloc_SparseMatrix();
00107 }
00108 
00109 const SparseMatrix& SparseMatrix::operator= (const SparseMatrix& mat) {
00110   // sanity check
00111   if (this==&mat)
00112     return *this;
00113 
00114   // free old stuff
00115   _dealloc_SparseMatrix();
00116 
00117   // copy rhs
00118   _copy_from_SparseMatrix(mat);
00119 
00120   // return self
00121   return *this;
00122 }
00123 
00124 double SparseMatrix::operator()(int row, int col) const {
00125   requireDebug((row>=0) && (row<_num_rows) && (col>=0) && (col<_num_cols),
00126           "SparseMatrix::operator(): Index out of range.");
00127   return (*_rows[row])(col);
00128 }
00129 
00130 void SparseMatrix::set(int row, int col, const double val, bool grow_matrix) {
00131   requireDebug(row>=0 && col>=0, "SparseMatrix::set: Invalid index.");
00132   if (grow_matrix) {
00133     ensure_num_rows(row+1);
00134     ensure_num_cols(col+1);
00135   } else {
00136     requireDebug(row<_num_rows && col<_num_cols, "SparseMatrix::set: Index out of range.");
00137   }
00138   _rows[row]->set(col, val);
00139 }
00140 
00141 void SparseMatrix::append_in_row(int row, int col,const double val) {
00142   requireDebug(row>=0 && col>=0 && row<_num_rows && col<_num_cols,
00143       "SparseMatrix::append_in_row: Index out of range.");
00144   _rows[row]->append(col, val);
00145 }
00146 
00147 int SparseMatrix::nnz() const {
00148   int nnz = 0;
00149   for (int row=0; row<_num_rows; row++) {
00150     nnz+=_rows[row]->nnz();
00151   }
00152   return nnz;
00153 }
00154 
00155 int SparseMatrix::max_nz() const {
00156   int max_nz = 0;
00157   for (int row=0; row<_num_rows; row++) {
00158     max_nz = max(max_nz, _rows[row]->nnz());
00159   }
00160   return max_nz;
00161 }
00162 
00163 void SparseMatrix::print(ostream& out) const {
00164   out << "%triples: (" << _num_rows << "x" << _num_cols << ", nnz:" << nnz() << ")" << endl;
00165   out.precision(12);
00166   for (int row=0; row<_num_rows; row++) {
00167     for (SparseVectorIter iter(*_rows[row]); iter.valid(); iter.next()) {
00168       double val;
00169       int col = iter.get(val);
00170       out << row << " " << col << " " << val << endl;
00171     }
00172   }
00173 }
00174 
00175 void SparseMatrix::print(const string& file_name) const {
00176   ofstream out(file_name.c_str());
00177   print(out);
00178   out.close();
00179 }
00180 
00181 void SparseMatrix::print_stats() const {
00182   cout << num_rows() << "x" << num_cols() << " nnz:" << nnz() << endl;
00183 }
00184 
00185 void SparseMatrix::print_pattern() const {
00186   for (int row=0; row<_num_rows; row++) {
00187     for (int col=0; col<_num_cols; col++) {
00188       double val = operator()(row, col);
00189       if (val!=0) {
00190         cout << "x";
00191       } else {
00192         cout << ".";
00193       }
00194     }
00195     cout << endl;
00196   }
00197 }
00198 
00199 void SparseMatrix::save_pattern_eps(const string& file_name) const {
00200   int x = num_cols();
00201   int y = num_rows();
00202   // find a scale factor that yields valid EPS coordinates
00203   int m = max(x,y);
00204   double scale = 1.;
00205   for (; scale*m >= 10000.; scale*=0.1);
00206   // create file
00207   ofstream out(file_name.c_str());
00208   out << "%!PS-Adobe-3.0 EPSF-3.0\n"
00209       "%%BoundingBox: 0 0 " << x*scale << " " << y*scale << "\n"
00210       "/BP{" << scale << " " << -scale << " scale 0 " << -y << " translate}bind def\n"
00211       "BP\n"
00212       "150 dict begin\n"
00213       "/D/dup cvx def/S/stroke cvx def\n"
00214       "/L/lineto cvx def/M/moveto cvx def\n"
00215       "/RL/rlineto cvx def/RM/rmoveto cvx def\n"
00216       "/GS/gsave cvx def/GR/grestore cvx def\n"
00217       "/REC{M 0 1 index RL 1 index 0 RL neg 0 exch RL neg 0 RL}bind def\n"
00218       "0 0 150 setrgbcolor\n"
00219       "0.01 setlinewidth\n";
00220   for (int row=0; row<_num_rows; row++) {
00221     for (SparseVectorIter iter(*_rows[row]); iter.valid(); iter.next()) {
00222       double val;
00223       int col = iter.get(val);
00224       out << "1 1 " << col << " " << row << " REC GS fill GR S" << endl;
00225     }
00226   }
00227   out.close();
00228 }
00229 
00230 const SparseVector& SparseMatrix::get_row(int row) const {
00231   requireDebug(row>=0 && row<_num_rows, "SparseMatrix::get_row: Index out of range.");
00232   return *_rows[row];
00233 }
00234 
00235 void SparseMatrix::set_row(int row, const SparseVector& new_row) {
00236   requireDebug(row>=0 && row<_num_rows, "SparseMatrix::set_row: Index out of range.");
00237   *_rows[row] = new_row;
00238 }
00239 
00240 void SparseMatrix::import_rows(int num_rows, int num_cols, SparseVector_p* rows) {
00241   _dealloc_SparseMatrix();
00242   _num_rows = num_rows;
00243   _num_cols = num_cols;
00244   _max_num_rows = num_rows;
00245   _max_num_cols = num_cols;
00246   _rows = new SparseVector_p[_max_num_rows];
00247   for (int row=0; row<_num_rows; row++) {
00248     _rows[row] = rows[row];
00249   }
00250 }
00251 
00252 void SparseMatrix::append_new_rows(int num) {
00253   requireDebug(num>=1, "SparseMatrix::append_new_rows: Cannot add less than one row.");
00254   int pos = _num_rows;
00255   if (_num_rows+num > _max_num_rows) {
00256     // automatic resizing, amortized cost O(1)
00257     int new_max_num_rows = max(2*_max_num_rows, _num_rows+num);
00258     SparseVector_p* new_rows = new SparseVector_p[new_max_num_rows];
00259     memcpy(new_rows, _rows, _num_rows*sizeof(SparseVector*));
00260     delete[] _rows;
00261     _max_num_rows = new_max_num_rows;
00262     _rows = new_rows;
00263   }
00264   for (int row=pos; row<=pos-1+num; row++) {
00265     _rows[row] = new SparseVector();
00266   }
00267   _num_rows += num;
00268 }
00269 
00270 void SparseMatrix::append_new_cols(int num) {
00271   requireDebug(num>=1, "SparseMatrix::append_new_cols: Cannot add less than one column.");
00272   if (_num_cols+num > _max_num_cols) {
00273     // this is actually used in OrderedSparseMatrix for the column translation table
00274     _max_num_cols = max(2*_max_num_cols, _num_cols+num);
00275   }
00276   _num_cols += num;
00277 }
00278 
00279 void SparseMatrix::ensure_num_rows(int num_rows) {
00280   requireDebug(num_rows>0, "SparseMatrix::ensure_num_rows: num_rows must be positive.");
00281   if (_num_rows < num_rows) {
00282     append_new_rows(num_rows - _num_rows);
00283   }
00284 }
00285 
00286 void SparseMatrix::ensure_num_cols(int num_cols) {
00287   requireDebug(num_cols>0, "SparseMatrix::ensure_num_cols: num_cols must be positive.");
00288   if (_num_cols < num_cols) {
00289     append_new_cols(num_cols - _num_cols);
00290   }
00291 }
00292 
00293 void SparseMatrix::remove_row() {
00294   requireDebug(_num_rows>0, "SparseMatrix::remove_row called on empty matrix.");
00295   // no need to worry about resizing _rows itself for this special case...
00296   delete _rows[_num_rows-1];
00297   _rows[_num_rows-1] = NULL;
00298   _num_rows--;
00299 }
00300 
00301 void SparseMatrix::apply_givens(int row, int col, double* c_givens, double* s_givens) {
00302   requireDebug(row>=0 && row<_num_rows && col>=0 && col<_num_cols, "SparseMatrix::apply_givens: index outside matrix.");
00303   requireDebug(row>col, "SparseMatrix::apply_givens: can only zero entries below the diagonal.");
00304   const SparseVector& row_top = *_rows[col];
00305   const SparseVector& row_bot = *_rows[row];
00306   double a = row_top(col);
00307   double b = row_bot(col);
00308   double c, s;
00309   givens(a, b, c, s);
00310   if (c_givens) *c_givens = c;
00311   if (s_givens) *s_givens = s;
00312 
00313   int n = row_bot.nnz() + row_top.nnz();
00314 
00315   SparseVector_p new_row_top = new SparseVector(n);
00316   SparseVector_p new_row_bot = new SparseVector(n);
00317   SparseVectorIter iter_top(row_top);
00318   SparseVectorIter iter_bot(row_bot);
00319   bool top_valid = iter_top.valid();
00320   bool bot_valid = iter_bot.valid();
00321   while (top_valid || bot_valid) {
00322     double val_top = 0.;
00323     double val_bot = 0.;
00324     int idx_top = (top_valid)?iter_top.get(val_top):-1;
00325     int idx_bot = (bot_valid)?iter_bot.get(val_bot):-1;
00326     int idx;
00327     if (idx_bot<0) {
00328       idx = idx_top;
00329     } else if (idx_top<0) {
00330       idx = idx_bot;
00331     } else {
00332       idx = min(idx_top, idx_bot);
00333     }
00334     if (top_valid) {
00335       if (idx==idx_top) {
00336         iter_top.next();
00337       } else {
00338         val_top = 0.;
00339       }
00340     }
00341     if (bot_valid) {
00342       if (idx==idx_bot) {
00343         iter_bot.next();
00344       } else {
00345         val_bot = 0.;
00346       }
00347     }
00348     double new_val_top = c*val_top - s*val_bot;
00349     double new_val_bot = s*val_top + c*val_bot;
00350     // remove numerically zero values to keep sparsity
00351     if (fabs(new_val_top) >= NUMERICAL_ZERO) {
00352       // append for O(1) operation - even O(log n) is too
00353       // slow here, because this is called extremely often!
00354       new_row_top->append(idx, new_val_top);
00355     }
00356     if (fabs(new_val_bot) >= NUMERICAL_ZERO) {
00357       new_row_bot->append(idx, new_val_bot);
00358     }
00359     top_valid = iter_top.valid();
00360     bot_valid = iter_bot.valid();
00361   }
00362 
00363   delete _rows[col];
00364   delete _rows[row];
00365 
00366   _rows[col] = new_row_top;
00367   _rows[row] = new_row_bot;
00368   _rows[row]->remove(col); // by definition, this entry is exactly 0
00369 }
00370 
00371 int SparseMatrix::triangulate_with_givens() {
00372   int count = 0;
00373   for (int row=0; row<_num_rows; row++) {
00374     while (true) {
00375       int col = _rows[row]->first();
00376       if (col >= row || col < 0) {
00377         break;
00378       }
00379       apply_givens(row, col);
00380       count++;
00381     }
00382   }
00383   return count;
00384 }
00385 
00386 const VectorXd operator*(const SparseMatrix& lhs, const VectorXd& rhs) {
00387   requireDebug(lhs.num_cols()==rhs.rows(), "SparseMatrix::operator* matrix and vector incompatible.");
00388   VectorXd res(lhs.num_rows());
00389   res.setZero();
00390   for (int row=0; row<lhs.num_rows(); row++) {
00391     for (SparseVectorIter iter(lhs.get_row(row)); iter.valid(); iter.next()) {
00392       double val;
00393       int col = iter.get(val);
00394       res(row) += val * rhs(col);
00395     }
00396   }
00397   return res;
00398 }
00399 
00400 VectorXd mul_SparseMatrixTrans_Vector(const SparseMatrix& lhs, const VectorXd& rhs) {
00401   requireDebug(lhs.num_rows()==rhs.rows(), "SparseMatrix::mul_SparseMatrixTrans_Vector matrix and vector incompatible.");
00402   VectorXd res(lhs.num_cols());
00403   res.setZero();
00404   for (int row=0; row<lhs.num_rows(); row++) {
00405     for (SparseVectorIter iter(lhs.get_row(row)); iter.valid(); iter.next()) {
00406       double val;
00407       int col = iter.get(val);
00408       res(col) += val * rhs(row);
00409     }
00410   }
00411   return res;
00412 }
00413 
00414 SparseMatrix sparseMatrix_of_matrix(const MatrixXd& m) {
00415   SparseMatrix s(m.rows(), m.cols());
00416   for (int r=0; r<m.rows(); r++) {
00417     for (int c=0; c<m.cols(); c++) {
00418       s.set(r, c, m(r,c));
00419     }
00420   }
00421   return s;
00422 }
00423 
00424 MatrixXd matrix_of_sparseMatrix(const SparseMatrix& s) {
00425   MatrixXd m(s.num_rows(), s.num_cols());
00426   for (int r=0; r<s.num_rows(); r++) {
00427     for (int c=0; c<s.num_cols(); c++) {
00428       m(r, c) = s(r,c);
00429     }
00430   }
00431   return m;
00432 }
00433 
00434 }
00435 


demo_lidar
Author(s): Ji Zhang
autogenerated on Sun Oct 5 2014 23:22:43