iSAM
SparseSystem.cpp
Go to the documentation of this file.
00001 
00028 #include <string>
00029 #include <cstring> // memset()
00030 #include <fstream>
00031 #include <iostream>
00032 #include <cmath>
00033 
00034 #include "isam/util.h"
00035 
00036 #include "isam/SparseSystem.h"
00037 
00038 using namespace std;
00039 using namespace Eigen;
00040 
00041 namespace isam {
00042 
00043 SparseSystem::SparseSystem(int num_rows, int num_cols) : OrderedSparseMatrix(num_rows, num_cols), _rhs(VectorXd(num_rows)) {
00044 }
00045 
00046 SparseSystem::SparseSystem(const SparseSystem& mat) : OrderedSparseMatrix(mat), _rhs(mat._rhs) {
00047 }
00048 
00049 SparseSystem::SparseSystem(const SparseSystem& mat, int num_rows, int num_cols, int first_row, int first_col) :
00050   OrderedSparseMatrix(mat, num_rows, num_cols, first_row, first_col), _rhs(mat._rhs.segment(first_row, num_rows)) {
00051 }
00052 
00053 SparseSystem::SparseSystem(int num_rows, int num_cols, SparseVector_p* rows, const VectorXd& rhs) :
00054   OrderedSparseMatrix(num_rows, num_cols, rows) {
00055   _rhs = rhs;
00056 }
00057 
00058 SparseSystem::~SparseSystem() {
00059 }
00060 
00061 const SparseSystem& SparseSystem::operator= (const SparseSystem& mat) {
00062   if (this==&mat)
00063     return *this;
00064 
00065   OrderedSparseMatrix::operator=(mat);
00066   _rhs = mat._rhs;
00067 
00068   return *this;
00069 }
00070 
00071 void SparseSystem::apply_givens(int row, int col, double* c_givens, double* s_givens) {
00072   double c, s;
00073   SparseMatrix::apply_givens(row, col, &c, &s);
00074   // modify rhs
00075   double r1 = _rhs(col);
00076   double r2 = _rhs(row);
00077   _rhs(col) = c*r1 - s*r2;
00078   _rhs(row) = s*r1 + c*r2;
00079 }
00080 
00081 void SparseSystem::append_new_rows(int num) {
00082   OrderedSparseMatrix::append_new_rows(num);
00083   _rhs.conservativeResize(_rhs.size() + num);
00084 }
00085 
00086 void SparseSystem::add_row(const SparseVector& new_row, double new_r) {
00087   ensure_num_cols(new_row.last()+1);
00088 
00089   append_new_rows(1);
00090   int row = num_rows() - 1;
00091   _rhs(row) = new_r;
00092   set_row(row, new_row);
00093 }
00094 
00095 int SparseSystem::add_row_givens(const SparseVector& new_row, double new_r) {
00096   // set new row (also translates according to current variable ordering)
00097   add_row(new_row, new_r);
00098   int count = 0;
00099 
00100   int row = num_rows() - 1; // last row
00101 
00102   int col = get_row(row).first(); // first entry to be zeroed
00103   while (col>=0 && col<row) { // stop when we reach the diagonal
00104     apply_givens(row, col);
00105     count++;
00106     col = get_row(row).first();
00107   }
00108   if (get_row(row).nnz()==0) {
00109     // need to remove the new row as it is now empty
00110     remove_row();
00111     // and the rhs needs to be cut accordingly
00112     VectorXd v = _rhs.segment(0, row); // temporary variable is necessary because of aliasing in Eigen
00113     _rhs = v;
00114   }
00115 
00116   return count;
00117 }
00118 
00119 VectorXd SparseSystem::solve() const {
00120   requireDebug(num_rows() >= num_cols(), "SparseSystem::solve: cannot solve system, not enough constraints");
00121   requireDebug(num_rows() == num_cols(), "SparseSystem::solve: system not triangular");
00122   int n = num_cols();
00123   VectorXd result(n);
00124   for (int row=n-1; row>=0; row--) {
00125     const SparseVector& rowvec = get_row(row);
00126     // start with rhs...
00127     double terms = _rhs(row);
00128     double diag;
00129 
00130     // ... and subtract already solved variables multiplied with respective coefficient from R
00131     // We assume that the SpareSystem is triangular
00132     SparseVectorIter iter(rowvec);
00133     iter.get(diag); // we can check return value it should be == row
00134     iter.next();
00135     for (; iter.valid(); iter.next()) {
00136       double v;
00137       int col = iter.get(v);
00138       terms = terms - result(col)*v;
00139     }
00140     // divide result by diagonal entry of R
00141     result(row) = terms / diag;
00142   }
00143   return result;
00144 }
00145 
00146 }
 All Classes Files Functions Variables