111 lines
3.9 KiB
C++
111 lines
3.9 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file GaussianMultifrontalSolver.h
|
|
* @brief
|
|
* @author Richard Roberts
|
|
* @created Oct 21, 2010
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/inference/GenericMultifrontalSolver.h>
|
|
#include <gtsam/linear/GaussianJunctionTree.h>
|
|
#include <gtsam/linear/GaussianBayesNet.h>
|
|
#include <gtsam/linear/GaussianFactorGraph.h>
|
|
#include <gtsam/linear/VectorValues.h>
|
|
|
|
#include <utility>
|
|
#include <vector>
|
|
|
|
namespace gtsam {
|
|
|
|
/** This solver uses multifrontal elimination to solve a GaussianFactorGraph,
|
|
* i.e. a sparse linear system. Underlying this is a junction tree, which is
|
|
* eliminated into a Bayes tree.
|
|
*
|
|
* The elimination ordering is "baked in" to the variable indices at this
|
|
* stage, i.e. elimination proceeds in order from '0'. A fill-reducing
|
|
* ordering is computed symbolically from the NonlinearFactorGraph, on the
|
|
* nonlinear side of gtsam. (To be precise, it is possible to permute an
|
|
* existing GaussianFactorGraph into a COLAMD ordering instead, this is done
|
|
* when computing marginals).
|
|
*
|
|
* The JunctionTree recursively produces a BayesTree<GaussianConditional>,
|
|
* on which this class calls optimize(...) to perform back-substitution.
|
|
*/
|
|
class GaussianMultifrontalSolver : GenericMultifrontalSolver<GaussianFactor, GaussianJunctionTree> {
|
|
|
|
protected:
|
|
|
|
typedef GenericMultifrontalSolver<GaussianFactor, GaussianJunctionTree> Base;
|
|
typedef boost::shared_ptr<const GaussianMultifrontalSolver> shared_ptr;
|
|
|
|
public:
|
|
|
|
/**
|
|
* Construct the solver for a factor graph. This builds the elimination
|
|
* tree, which already does some of the symbolic work of elimination.
|
|
*/
|
|
GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>& factorGraph);
|
|
|
|
/**
|
|
* Named constructor that returns a shared_ptr. This builds the junction
|
|
* tree, which already does some of the symbolic work of elimination.
|
|
*/
|
|
static shared_ptr Create(const FactorGraph<GaussianFactor>& factorGraph);
|
|
|
|
/**
|
|
* Return a new solver that solves the given factor graph, which must have
|
|
* the *same structure* as the one this solver solves. For some solvers this
|
|
* is more efficient than constructing the solver from scratch. This can be
|
|
* used in cases where the numerical values of the linear problem change,
|
|
* e.g. during iterative nonlinear optimization.
|
|
*/
|
|
shared_ptr update(const FactorGraph<GaussianFactor>& factorGraph) const;
|
|
|
|
/**
|
|
* Eliminate the factor graph sequentially. Uses a column elimination tree
|
|
* to recursively eliminate.
|
|
*/
|
|
BayesTree<GaussianConditional>::shared_ptr eliminate() const;
|
|
|
|
/**
|
|
* Compute the least-squares solution of the GaussianFactorGraph. This
|
|
* eliminates to create a BayesNet and then back-substitutes this BayesNet to
|
|
* obtain the solution.
|
|
*/
|
|
VectorValues::shared_ptr optimize() const;
|
|
|
|
/**
|
|
* Compute the marginal Gaussian density over a variable, by integrating out
|
|
* all of the other variables. This function returns the result as an upper-
|
|
* triangular R factor and right-hand-side, i.e. a GaussianConditional with
|
|
* R*x = d. To get a mean and covariance matrix, use marginalStandard(...)
|
|
*/
|
|
GaussianFactor::shared_ptr marginal(Index j) const;
|
|
|
|
/**
|
|
* Compute the marginal Gaussian density over a variable, by integrating out
|
|
* all of the other variables. This function returns the result as a mean
|
|
* vector and covariance matrix. Compared to marginalCanonical, which
|
|
* returns a GaussianConditional, this function back-substitutes the R factor
|
|
* to obtain the mean, then computes \Sigma = (R^T * R)^-1.
|
|
*/
|
|
std::pair<Vector, Matrix> marginalStandard(Index j) const;
|
|
|
|
};
|
|
|
|
}
|
|
|
|
|