iSAM
SparseVector.h
00001 
00028 #pragma once
00029 
00030 #include <Eigen/Dense>
00031 
00032 #include "isam/util.h"
00033 
00034 namespace isam {
00035 
00036 class SparseVector {
00037   int _nnz;
00038   int _nnz_max;
00039   int* _indices;
00040   double* _values;
00041 
00046   void _copy_from(const SparseVector& vec);
00047 
00051   void _dealloc();
00052 
00057   int _search(int idx) const;
00058 
00063   void _resize(int new_nnz_max);
00064 
00065 public:
00069   SparseVector();
00070 
00071   SparseVector(int nnz_max) : _nnz(0), _nnz_max(nnz_max) {
00072     _indices = new int[_nnz_max];
00073     _values = new double[_nnz_max];
00074   }
00075   
00080   SparseVector(const SparseVector& vec);
00081 
00088   SparseVector(const SparseVector& vec, int num, int first = 0);
00089 
00096   SparseVector(int* indices, double* values, int nnz);
00097 
00101   virtual ~SparseVector();
00102 
00108   const SparseVector& operator= (const SparseVector& vec);
00109 
00115   double operator()(int idx) const;
00116 
00122   void copy_raw(int* indices, double* values) const;
00123 
00130   inline void append(int idx, const double val = 0.) {
00131     requireDebug(_nnz==0 || _indices[_nnz-1] < idx, "SparseVector::append: index has to be after last entry");
00132 
00133     if (_nnz+1 > _nnz_max) {
00134       // automatic resizing, amortized cost O(1)
00135       int new_nnz_max = _nnz_max*2;
00136       _resize(new_nnz_max);
00137     }
00138     // insert new entry
00139     _indices[_nnz] = idx;
00140     _values[_nnz] = val;
00141     _nnz++;
00142   }
00143 
00150   bool set(int idx, const double val = 0.);
00151   
00152   
00160   bool set(int idx, const Eigen::VectorXd& vals);
00161 
00166   void remove(int idx);
00167 
00172   int first() const;
00173 
00178   int last() const;
00179 
00185   void add_entries(int num, int pos);
00186 
00190   void print() const;
00191 
00192   inline int nnz() const {
00193     return _nnz;
00194   }
00195 
00196   friend class SparseVectorIter;
00197 };
00198 
00199 class SparseVectorIter {
00200   const SparseVector& s;
00201   int index;
00202 public:
00207   inline SparseVectorIter(const SparseVector& sv) : s(sv), index(0) {}
00208 
00213   inline bool valid() const {
00214     return (index < s._nnz);
00215   }
00216 
00221   inline int get() const {
00222     requireDebug(index < s._nnz, "SparseVectorIter::get(): Index out of range.");
00223     int tmp = s._indices[index];
00224     return tmp;
00225   }
00226 
00232   inline int get(double& val) const {
00233     requireDebug(index < s._nnz, "SparseVectorIter::get(): Index out of range.");
00234     val = s._values[index];
00235     return s._indices[index];
00236   }
00237 
00242   inline double get_val() const {
00243     requireDebug(index < s._nnz, "SparseVectorIter::get_val(): Index out of range.");
00244     return s._values[index];
00245   }
00246 
00250   inline void next() {
00251     if (index < s._nnz) {
00252       index++;
00253     }
00254   }
00255 };
00256 
00257 }
00258 
 All Classes Files Functions Variables