covariance.cpp
Go to the documentation of this file.
00001 
00029 #include <vector>
00030 #include <utility> // pair
00031 
00032 #include "isam/covariance.h"
00033 #include "isam/util.h"
00034 
00035 using namespace std;
00036 using namespace Eigen;
00037 
00038 namespace isam {
00039 
00040 void prepare(const SparseMatrix& R, CovarianceCache& cache) {
00041   // reset hash table
00042   cache.entries.clear();
00043   // speedup row access
00044   int n = R.num_cols();
00045   cache.rows.resize(n);
00046   cache.rows_valid.resize(n);
00047   cache.current_valid++; // invalidate previous entries
00048   // wrapped back to 0? then we do have to reset the table
00049   if (cache.current_valid==0) {
00050     for (int i=0; i<n; i++) {
00051       cache.rows_valid[i] = 0;
00052     }
00053     cache.current_valid = 1;
00054   }
00055   // precalculate inverses
00056   cache.diag.resize(n);
00057   for (int i=0; i<n; i++) {
00058     cache.diag[i] = 1. / R(i,i);
00059   }
00060   // stats
00061   cache.num_calc = 0;
00062 }
00063 
00064 const SparseVector& get_row(const SparseMatrix& R, CovarianceCache& cache, int i) {
00065   if (cache.rows_valid[i] != cache.current_valid) {
00066     // retrieve and store row
00067     cache.rows[i] = R.get_row(i);
00068     cache.rows_valid[i] = cache.current_valid;
00069   }
00070   return cache.rows[i];
00071 }
00072 
00073 // for recursive call
00074 double recover(const SparseMatrix& R, CovarianceCache& cache, int n, int i, int l);
00075 
00076 inline double sum_j(const SparseMatrix& R, CovarianceCache& cache, int n, int l, int i) {
00077   double sum = 0;
00078   for (SparseVectorIter iter(get_row(R, cache, i)); iter.valid(); iter.next()) {
00079     double rij;
00080     int j = iter.get(rij);
00081     if (j!=i) {
00082       double lj;
00083       if (j>l) {
00084         lj = recover(R, cache, n, l, j);
00085       } else {
00086         lj = recover(R, cache, n, j, l);
00087       }
00088       sum += rij * lj;
00089     }
00090   }
00091   return sum;
00092 }
00093 
00094 double recover(const SparseMatrix& R, CovarianceCache& cache, int n, int i, int l) {
00095   if (i>l) {int tmp=i; i=l; l=tmp;}
00096   int id = i*n + l; // flatten index for hash table
00097   umap::iterator it = cache.entries.find(id);
00098   double res;
00099   if (it == cache.entries.end()) {
00100     //printf("calculating entry %i,%i\n", i, l);
00101     // need to calculate entry
00102     if (i==l) {
00103       res = cache.diag[l] * (cache.diag[l] - sum_j(R, cache, n, l, l));
00104     } else {
00105       res = -sum_j(R, cache, n, l, i) * cache.diag[i];
00106     }
00107     // insert into hash
00108     pair<int, double> entry(id, res);
00109     cache.entries.insert(entry);
00110     cache.num_calc++;
00111   } else {
00112     //printf("retrieved entry %i,%i\n", i, l);
00113     // retrieve value from hash
00114     res = it->second;
00115   }
00116   return res;
00117 }
00118 
00119 list<MatrixXd> cov_marginal(const SparseMatrix& R, CovarianceCache& cache,
00120                             const index_lists_t& index_lists, bool debug, int step) {
00121   prepare(R, cache);
00122   list<MatrixXd> Cs;
00123 
00124   // debugging
00125   int requested = 0;
00126   double t0 = tic();
00127 
00128   for (unsigned int i=0; i<index_lists.size(); i++) {
00129 
00130     const vector<int>& indices = index_lists[i];
00131     unsigned int n_indices = indices.size();
00132     MatrixXd C(n_indices, n_indices);
00133     // recover entries of marginal cov
00134     int n = R.num_cols();
00135     for (int r=n_indices-1; r>=0; r--) {
00136       for (int c=n_indices-1; c>=r; c--) {
00137         C(r,c) = recover(R, cache, n, indices[r], indices[c]);
00138       }
00139     }
00140     for (unsigned int r=1; r<n_indices; r++) {
00141       for (unsigned int c=0; c<r; c++) {
00142         C(r,c) = C(c,r);
00143       }
00144     }
00145     Cs.push_back(C);
00146     requested += indices.size()*indices.size();
00147   }
00148 
00149   if (debug) {
00150     double time = toc(t0);
00151     // timing
00152     printf("cov: %i calculated for %i requested in %f seconds\n",
00153            cache.num_calc, requested, time);
00154     if (step>=0) {
00155       // stats for gnuplot
00156       printf("ggg %i %i %i %i %i %i %f ",
00157              step,
00158              R.num_cols(), // side length
00159              R.num_cols()*R.num_cols(), // #entries dense
00160              R.nnz(), // #entries sparse
00161              cache.num_calc, // #entries calculated
00162              requested, // #entries requested
00163              time); // #execution time
00164     }
00165   }
00166 
00167   return Cs;
00168 }
00169 
00170 list<double> cov_marginal(const SparseMatrix& R, CovarianceCache& cache,
00171                           const entry_list_t& entry_list) {
00172   prepare(R, cache);
00173   list<double> entries;
00174 
00175   int n = R.num_cols();
00176   for (unsigned int i=0; i<entry_list.size(); i++) {
00177     const pair<int, int>& index = entry_list[i];
00178     double entry = recover(R, cache, n, index.first, index.second);
00179     entries.push_back(entry);
00180   }
00181 
00182   return entries;
00183 }
00184 
00185 }


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