SparseVector.cpp
Go to the documentation of this file.
00001 
00029 #include <string>
00030 #include <cstring> // memmove()
00031 #include <iostream>
00032 #include <map>
00033 
00034 #include "isam/util.h"
00035 
00036 #include "isam/SparseVector.h"
00037 
00038 using namespace std;
00039 using namespace Eigen;
00040 
00041 namespace isam {
00042 
00043 
00044 // will significantly influence memory usage of large, but very sparse
00045 // matrices; also influence on execution time if chosen too small (10)
00046 const int INITIAL_ENTRIES = 50;
00047 
00048 void SparseVector::_copy_from(const SparseVector& vec) {
00049   _nnz = vec._nnz;
00050   _nnz_max = vec._nnz_max;
00051 
00052   _indices = new int[_nnz_max];
00053   memcpy(_indices, vec._indices, _nnz*sizeof(int));
00054   _values = new double[_nnz_max];
00055   memcpy(_values, vec._values, _nnz*sizeof(double));
00056 }
00057 
00058 void SparseVector::_dealloc() {
00059   if (_indices != NULL) {
00060     delete[] _indices;
00061     _indices = NULL;
00062   }
00063   if (_values != NULL) {
00064     delete[] _values;
00065     _values = NULL;
00066   }
00067 }
00068 
00069 int SparseVector::_search(int idx) const {
00070 #if 0
00071   // naive implementation
00072   int n = 0;
00073   while (n<_nnz && (_indices[n] < idx)) {
00074     n++;
00075   }
00076 #else
00077   // binary search implementation
00078   int n = 0;
00079   if (_nnz>0) {
00080     int n0 = 0;
00081     int n1 = _nnz-1;
00082     while (n1-n0>1) {
00083       n = n0+((n1-n0)>>1);
00084       if (_indices[n] <= idx) {
00085         n0 = n;
00086       } else {
00087         n1 = n;
00088       }
00089     }
00090     if (_indices[n1] < idx) {
00091       n = n1+1;
00092     } else if ((_indices[n1] == idx) || (_indices[n0] < idx)) {
00093       n = n1;
00094     } else {
00095       n = n0;
00096     }
00097   }
00098 #endif
00099   return n;
00100 }
00101 
00102 void SparseVector::_resize(int new_nnz_max) {
00103   int* new_indices = new int[new_nnz_max];
00104   memcpy(new_indices, _indices, _nnz*sizeof(int));
00105   delete[] _indices;
00106   _indices = new_indices;
00107   double* new_values = new double[new_nnz_max];
00108   memcpy(new_values, _values, _nnz*sizeof(double));
00109   delete[] _values;
00110   _values = new_values;
00111   _nnz_max = new_nnz_max;
00112 }
00113 
00114 SparseVector::SparseVector() {
00115   _nnz = 0;
00116   _nnz_max = INITIAL_ENTRIES;
00117 
00118   _indices = new int[_nnz_max];
00119   _values = new double[_nnz_max];
00120 }
00121 
00122 SparseVector::SparseVector(const SparseVector& vec) {
00123   _copy_from(vec);
00124 }
00125 
00126 SparseVector::SparseVector(const SparseVector& vec, int num, int first) {
00127   // first have to figure out how many entries in the given range
00128   _nnz = 0;
00129   for (int i=0; i<vec._nnz; i++) {
00130     int idx = vec._indices[i];
00131     if (idx>=first && idx<first+num) {
00132       _nnz++;
00133     }
00134   }
00135   // allocate memory accordingly
00136   _nnz_max = _nnz;
00137   _indices = new int[_nnz_max];
00138   _values = new double[_nnz_max];
00139   // copy data over, renumber indices!
00140   int n = 0;
00141   for (int i=0; i<vec._nnz; i++) {
00142     int idx = vec._indices[i];
00143     if (idx>=first && idx<first+num) {
00144       _indices[n] = vec._indices[i]-first;
00145       _values[n] = vec._values[i];
00146       n++;
00147     }
00148   }
00149 }
00150 
00151 SparseVector::SparseVector(int* indices, double* values, int nnz) {
00152   _nnz = nnz;
00153   _nnz_max = nnz;
00154 
00155   _indices = new int[_nnz_max];
00156   _values = new double[_nnz_max];
00157 
00158   memcpy(_indices, indices, nnz*sizeof(int));
00159   memcpy(_values, values, nnz*sizeof(double));
00160 }
00161 
00162 SparseVector::~SparseVector() {
00163   _dealloc();
00164 }
00165 
00166 const SparseVector& SparseVector::operator= (const SparseVector& vec) {
00167   // sanity check
00168   if (this==&vec) {
00169     return *this;
00170   }
00171 
00172   // free old stuff
00173   _dealloc();
00174 
00175   // copy rhs
00176   _copy_from(vec);
00177 
00178   // return self
00179   return *this;
00180 }
00181 
00182 double SparseVector::operator()(int idx) const {
00183   int n = 0;
00184   n = _search(idx);
00185   if (n<_nnz && _indices[n] == idx) {
00186     return _values[n];
00187   } else {
00188     return 0.;
00189   }
00190 }
00191 
00192 void SparseVector::copy_raw(int* indices, double* values) const {
00193   memcpy(indices, _indices, _nnz*sizeof(int));
00194   memcpy(values, _values, _nnz*sizeof(double));
00195 }
00196 
00197 bool SparseVector::set(int idx, const double val) {
00198   VectorXd tmp(1);
00199   tmp << val;
00200   return set(idx, tmp);
00201 }
00202 
00203 bool SparseVector::set(int idx, const VectorXd& vals) {
00204   bool created_entry = false;
00205   int n = 0;
00206   int c = vals.rows();
00207   
00208   // First check if we can append
00209   if (_nnz > 0 && idx > _indices[_nnz-1]) {
00210     n = _nnz;
00211   } else {
00212     n = _search(idx);  // search closest entry
00213   }
00214   
00215   if (n<_nnz && _indices[n] == idx) {
00216     // entry exists? then overwrite!
00217     // BIG ASSUMPTION when writing multiple values:
00218     // they either all exist or they don't
00219     for (int i=0;i<c;i++) {
00220       _values[n+i] = vals(i);
00221     }
00222   } else {
00223     // new entries have to be created
00224     created_entry = true;
00225     // enough space left?
00226     if (_nnz+c > _nnz_max) {
00227       // automatic resizing, amortized cost O(1)
00228       int new_nnz_max = _nnz_max*2 + c;
00229       _resize(new_nnz_max);
00230     }
00231     // need to make space in the middle?
00232     if (n<_nnz) {
00233       memmove(&(_indices[n+c]), &(_indices[n]), (_nnz-n)*sizeof(int));
00234       memmove(&(_values[n+c]), &(_values[n]), (_nnz-n)*sizeof(double));
00235     }
00236     // insert new entry
00237     _nnz+=c;
00238     for (int i=0;i<c;i++) {
00239       _indices[n+i] = idx+i;
00240       _values[n+i] = vals(i);
00241     }
00242   }
00243 
00244   return created_entry;
00245 }
00246 
00247 void SparseVector::remove(int idx) {
00248   int n = 0;
00249   while (n<_nnz && (_indices[n] < idx)) {
00250     n++;
00251   }
00252   if (n<_nnz && _indices[n] == idx) {
00253     if (n!=_nnz-1) {
00254       // move all following entries
00255       memmove(&(_indices[n]), &(_indices[n+1]), (_nnz-n-1)*sizeof(int));
00256       memmove(&(_values[n]), &(_values[n+1]), (_nnz-n-1)*sizeof(double));
00257     }
00258     _indices[_nnz-1] = 0; // keep all unused entries at 0
00259     _nnz--;
00260 
00261     // potentially have to release memory; note that we require a quarter
00262     // to provide some threshold between reserving more and releasing it again
00263     int quarter = _nnz_max/4;
00264     if (INITIAL_ENTRIES < quarter && _nnz < quarter) {
00265       int new_nnz_max = _nnz_max/2;
00266       _resize(new_nnz_max);
00267     }
00268   }
00269 }
00270 
00271 int SparseVector::first() const {
00272   if (_nnz > 0) {
00273     return _indices[0];
00274   } else {
00275     return -1;
00276   }
00277 }
00278 
00279 int SparseVector::last() const {
00280   if (_nnz > 0) {
00281     return _indices[_nnz-1];
00282   } else {
00283     return -1;
00284   }
00285 }
00286 
00287 void SparseVector::add_entries(int num, int pos) {
00288   for (int i=0; i<_nnz; i++) {
00289     if (_indices[i] >= pos) {
00290       _indices[i] += num;
00291     }
00292   }
00293 }
00294 
00295 void SparseVector::print() const {
00296   cout << "%Vector: nnz=" << _nnz << endl;
00297   for (int i=0; i<_nnz; i++) {
00298     cout << _indices[i];
00299     cout << ": " << _values[i];
00300     cout << endl;
00301   }
00302 }
00303 
00304 }
00305 


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