Covariances.h

Go to the documentation of this file.

#pragma once

#include <list>
#include <map>
#include <Eigen/Dense>

#include "SparseSystem.h"
#include "Node.h"
#include "covariance.h"

namespace isam {

class Slam;

class Covariances {
private:
  // either directly coupled to a Slam object...
  Slam* _slam;

  // ...or we operate on a copy of the relevant data from a Slam object
  SparseSystem _R;
  std::map<Node*, std::pair<int, int> > _index;

  mutable CovarianceCache _cache;

  // utility function for _index
  int get_start(Node* node) const;
  int get_dim(Node* node) const;

  // only used for cloning below
  Covariances(Slam& slam);

public:

  Covariances(Slam* slam) : _slam(slam), _R(1,1) {}

  virtual ~Covariances() {};

  virtual Covariances clone() const {
    return Covariances(*_slam);
  }

  typedef std::list<std::list<Node*> > node_lists_t;
  typedef std::list<std::pair<Node*, Node*> > node_pair_list_t;

  virtual std::list<Eigen::MatrixXd> marginal(const node_lists_t& node_lists) const;

  virtual Eigen::MatrixXd marginal(const std::list<Node*>& nodes) const;

  virtual std::list<Eigen::MatrixXd> access(const node_pair_list_t& node_pair_list) const;

};

}