00001
00029 #include <vector>
00030 #include <utility>
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
00042 cache.entries.clear();
00043
00044 int n = R.num_cols();
00045 cache.rows.resize(n);
00046 cache.rows_valid.resize(n);
00047 cache.current_valid++;
00048
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
00056 cache.diag.resize(n);
00057 for (int i=0; i<n; i++) {
00058 cache.diag[i] = 1. / R(i,i);
00059 }
00060
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
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
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;
00097 umap::iterator it = cache.entries.find(id);
00098 double res;
00099 if (it == cache.entries.end()) {
00100
00101
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
00108 pair<int, double> entry(id, res);
00109 cache.entries.insert(entry);
00110 cache.num_calc++;
00111 } else {
00112
00113
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
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
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
00152 printf("cov: %i calculated for %i requested in %f seconds\n",
00153 cache.num_calc, requested, time);
00154 if (step>=0) {
00155
00156 printf("ggg %i %i %i %i %i %i %f ",
00157 step,
00158 R.num_cols(),
00159 R.num_cols()*R.num_cols(),
00160 R.nnz(),
00161 cache.num_calc,
00162 requested,
00163 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 }