covariance.h

Go to the documentation of this file.

#pragma once

#include <vector>
#include <utility> // pair
#include <list>
#include <Eigen/Dense>

#define USE_TR1
#ifdef USE_TR1
#include <tr1/unordered_map>
#else
#include "boost/unordered_map.hpp"
#endif

#include "SparseMatrix.h"

namespace isam {

#ifdef USE_TR1
#include <tr1/unordered_map>
typedef std::tr1::unordered_map<int, double> umap;
#else
typedef boost::unordered_map<int, double> umap;
#endif

class CovarianceCache {
public:
  umap entries;
  // precalculated diagonal inverses
  std::vector<double> diag;
  // recovering rows is expensive, buffer results
  std::vector<SparseVector> rows;
  // avoid having to cleanup buffers each time by explicitly marking entries as valid
  std::vector<unsigned int> rows_valid;
  // avoid having to cleanup valid entries by using different indices each time
  unsigned int current_valid;
  // stats
  int num_calc;

  CovarianceCache () {
    current_valid = 1;
  }
};

typedef std::vector< std::vector<int> > index_lists_t;
typedef std::vector< std::pair<int, int> > entry_list_t;

std::list<Eigen::MatrixXd> cov_marginal(const SparseMatrix& R, CovarianceCache& cache,
                                        const index_lists_t& index_lists,
                                        bool debug=false, int step=-1);

std::list<double> cov_marginal(const SparseMatrix& R, CovarianceCache& cache,
                               const entry_list_t& entry_list);

}