Marginals.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#pragma once

#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>

namespace gtsam {

class JointMarginal;

class GTSAM_EXPORT Marginals {

public:

  enum Factorization {
    CHOLESKY,
    QR
  };

protected:

  GaussianFactorGraph graph_;
  Values values_;
  Factorization factorization_;
  GaussianBayesTree bayesTree_;

public:

  Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);

  void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;

  Matrix marginalCovariance(Key variable) const;

  Matrix marginalInformation(Key variable) const;

  JointMarginal jointMarginalCovariance(const std::vector<Key>& variables) const;

  JointMarginal jointMarginalInformation(const std::vector<Key>& variables) const;
};

class GTSAM_EXPORT JointMarginal {

protected:
  SymmetricBlockMatrix blockMatrix_;
  std::vector<size_t> keys_;
  FastMap<Key, size_t> indices_;

public:
  typedef SymmetricBlockMatrix::constBlock Block;

  Block operator()(Key iVariable, Key jVariable) const {
    return blockMatrix_(indices_.at(iVariable), indices_.at(jVariable)); }

  Block at(Key iVariable, Key jVariable) const {
    return (*this)(iVariable, jVariable); }

  Eigen::SelfAdjointView<const Matrix, Eigen::Upper> fullMatrix() const { return blockMatrix_.matrix(); }

  void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;

protected:
  JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const std::vector<Key>& keys) :
    blockMatrix_(dims, fullMatrix), keys_(keys), indices_(Ordering(keys).invert()) {}

  friend class Marginals;

};

} /* namespace gtsam */