diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 06f9fd6cf..070fa89c9 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -39,6 +39,10 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, } Matrix Marginals::marginalCovariance(Key variable) const { + return marginalInformation(variable).inverse(); +} + +Matrix Marginals::marginalInformation(Key variable) const { // Get linear key Index index = ordering_[variable]; @@ -50,18 +54,16 @@ Matrix Marginals::marginalCovariance(Key variable) const { marginalFactor = bayesTree_.marginalFactor(index, EliminateQR); // Get information matrix (only store upper-right triangle) - Matrix info; if(typeid(*marginalFactor) == typeid(JacobianFactor)) { JacobianFactor::constABlock A = static_cast(*marginalFactor).getA(); - info = A.transpose() * A; // Compute A'A + return A.transpose() * A; // Compute A'A } else if(typeid(*marginalFactor) == typeid(HessianFactor)) { const HessianFactor& hessian = static_cast(*marginalFactor); const size_t dim = hessian.getDim(hessian.begin()); - info = hessian.info().topLeftCorner(dim,dim).selfadjointView(); // Take the non-augmented part of the information matrix + return hessian.info().topLeftCorner(dim,dim).selfadjointView(); // Take the non-augmented part of the information matrix + } else { + throw runtime_error("Internal error: Marginals::marginalInformation expected either a JacobianFactor or HessianFactor"); } - - // Compute covariance - return info.inverse(); } } /* namespace gtsam */ diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 48acc007e..dde199ca0 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -25,21 +25,33 @@ namespace gtsam { /** - * A class for computing marginals in a NonlinearFactorGraph + * A class for computing Gaussian marginals of variables in a NonlinearFactorGraph */ class Marginals { public: + /** The linear factorization mode - either CHOLESKY (faster and suitable for most problems) or QR (slower but more numerically stable for poorly-conditioned problems). */ enum Factorization { CHOLESKY, QR }; + /** Construct a marginals class. + * @param graph The factor graph defining the full joint density on all variables. + * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer). + * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). + */ Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY); + /** Compute the marginal covariance of a single variable */ Matrix marginalCovariance(Key variable) const; + /** Compute the marginal information matrix of a single variable. You can + * use LLt(const Matrix&) or RtR(const Matrix&) to obtain the square-root information + * matrix. */ + Matrix marginalInformation(Key variable) const; + protected: GaussianFactorGraph graph_;