iSAM
Covariances.cpp
Go to the documentation of this file.
00001 
00028 #include <vector>
00029 #include <list>
00030 #include <Eigen/Dense>
00031 
00032 #include "isam/covariance.h"
00033 #include "isam/Slam.h"
00034 
00035 #include "isam/Covariances.h"
00036 
00037 using namespace std;
00038 using namespace Eigen;
00039 
00040 namespace isam {
00041 
00042 Covariances::Covariances(Slam& slam) : _slam(NULL), _R(slam._R) {
00043   // update pointers into matrix before making a copy
00044   slam.update_starts();
00045   const list<Node*>& nodes = slam.get_nodes();
00046   for (list<Node*>::const_iterator it = nodes.begin(); it!=nodes.end(); it++) {
00047     Node* node = *it;
00048     int start = node->_start;
00049     int dim = node->dim();
00050     _index[node] = make_pair(start, dim);
00051   }  
00052 }
00053 
00054 int Covariances::get_start(Node* node) const {
00055   if (_slam) {
00056     return node->_start;
00057   } else {
00058     return _index.find(node)->second.first;
00059   }
00060 }
00061 
00062 int Covariances::get_dim(Node* node) const {
00063   if (_slam) {
00064     return node->_dim;
00065   } else {
00066     return _index.find(node)->second.second;
00067   }
00068 }
00069 
00070 list<MatrixXd> Covariances::marginal(const node_lists_t& node_lists) const {
00071   const SparseSystem& R = (_slam==NULL) ? _R : _slam->_R;
00072   if (_slam) {
00073     _slam->update_starts();
00074   }
00075 
00076   vector < vector<int> > index_lists(node_lists.size());
00077 
00078   if (R.num_rows()>1) { // skip if _R not calculated yet (eg. before batch step)
00079     int n=0;
00080     const int* trans = R.a_to_r();
00081     for (node_lists_t::const_iterator it_list = node_lists.begin();
00082         it_list != node_lists.end();
00083         it_list++, n++) {
00084       // assemble list of indices
00085       vector<int> indices;
00086       for (list<Node*>::const_iterator it = it_list->begin(); it!=it_list->end(); it++) {
00087         Node* node = *it;
00088         int start = get_start(node);
00089         int dim = get_dim(node);
00090         for (int i=0; i<dim; i++) {
00091           index_lists[n].push_back(trans[start+i]);
00092         }
00093       }
00094     }
00095     return cov_marginal(R, _cache, index_lists);
00096   }
00097   list<MatrixXd> empty_list;
00098   return empty_list;
00099 }
00100 
00101 MatrixXd Covariances::marginal(const list<Node*>& nodes) const {
00102   node_lists_t node_lists;
00103   node_lists.push_back(nodes);
00104   return marginal(node_lists).front();
00105 }
00106 
00107 list<MatrixXd> Covariances::access(const node_pair_list_t& node_pair_list) const {
00108   const SparseSystem& R = (_slam==NULL) ? _R : _slam->_R;
00109   if (_slam) {
00110     _slam->update_starts();
00111   }
00112 
00113   if (R.num_rows()>1) { // skip if _R not calculated yet (eg. before batch step)
00114 
00115     // count how many entries in requested blocks
00116     int num = 0;
00117     for (node_pair_list_t::const_iterator it = node_pair_list.begin();
00118          it != node_pair_list.end(); it++) {
00119       num += get_dim(it->first) * get_dim(it->second);
00120     }
00121 
00122     // request individual covariance entries
00123     vector < pair<int, int> > index_list(num);
00124     const int* trans = R.a_to_r();
00125     int n = 0;
00126     for (node_pair_list_t::const_iterator it = node_pair_list.begin();
00127          it != node_pair_list.end(); it++) {
00128       int start_r = get_start(it->first);
00129       int start_c = get_start(it->second);
00130       int dim_r = get_dim(it->first);
00131       int dim_c = get_dim(it->second);
00132       for (int r=0; r<dim_r; r++) {
00133         for (int c=0; c<dim_c; c++) {
00134           index_list[n] = make_pair(trans[start_r + r], trans[start_c + c]);
00135           n++;
00136         }
00137       }
00138     }
00139     list<double> covs = cov_marginal(R, _cache, index_list);
00140 
00141     // assemble into block matrices
00142     list<MatrixXd> result;
00143     list<double>::iterator it_cov = covs.begin();
00144     for (node_pair_list_t::const_iterator it = node_pair_list.begin();
00145          it != node_pair_list.end(); it++) {
00146       int dim_r = get_dim(it->first);
00147       int dim_c = get_dim(it->second);
00148       MatrixXd matrix(dim_r, dim_c);
00149       for (int r=0; r<dim_r; r++) {
00150         for (int c=0; c<dim_c; c++) {
00151           matrix(r,c) = *it_cov;
00152           it_cov++;
00153         }
00154       }
00155       result.push_back(matrix);
00156     }
00157     return result;
00158   }
00159   list<MatrixXd> empty_list;
00160   return empty_list;
00161 }
00162 
00163 }
 All Classes Files Functions Variables