Added unordered GaussianBayesTree
Richard Roberts
parent
79ce96dceb
commit
57f682998c
|
|
@ -17,77 +17,111 @@
|
|||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/base/treeTraversal-inst.h>
|
||||
#include <gtsam/inference/BayesTreeUnordered-inst.h>
|
||||
#include <gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h>
|
||||
#include <gtsam/linear/GaussianBayesTreeUnordered.h>
|
||||
#include <gtsam/linear/GaussianFactorGraphUnordered.h>
|
||||
#include <gtsam/linear/GaussianBayesNetUnordered.h>
|
||||
#include <gtsam/linear/VectorValuesUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues optimize(const GaussianBayesTree& bayesTree) {
|
||||
VectorValues result = *allocateVectorValues(bayesTree);
|
||||
optimizeInPlace(bayesTree, result);
|
||||
return result;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
namespace internal
|
||||
{
|
||||
/* ************************************************************************* */
|
||||
/** Pre-order visitor for back-substitution in a Bayes tree. The visitor function operator()()
|
||||
* optimizes the clique given the solution for the parents, and returns the solution for the
|
||||
* clique's frontal variables. In addition, it adds the solution to a global collected
|
||||
* solution that will finally be returned to the user. The reason we pass the individual
|
||||
* clique solutions between nodes is to avoid log(n) lookups over all variables, they instead
|
||||
* then are only over a node's parent variables. */
|
||||
struct OptimizeClique
|
||||
{
|
||||
VectorValuesUnordered collectedResult;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) {
|
||||
internal::optimizeInPlace<GaussianBayesTree>(bayesTree.root(), result);
|
||||
}
|
||||
VectorValuesUnordered operator()(
|
||||
const GaussianBayesTreeCliqueUnordered::shared_ptr& clique,
|
||||
const VectorValuesUnordered& parentSolution)
|
||||
{
|
||||
// parents are assumed to already be solved and available in result
|
||||
VectorValuesUnordered cliqueSolution = clique->conditional()->solve(parentSolution);
|
||||
collectedResult.insert(cliqueSolution);
|
||||
return cliqueSolution;
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues optimizeGradientSearch(const GaussianBayesTree& bayesTree) {
|
||||
gttic(Allocate_VectorValues);
|
||||
VectorValues grad = *allocateVectorValues(bayesTree);
|
||||
gttoc(Allocate_VectorValues);
|
||||
/* ************************************************************************* */
|
||||
double logDeterminant(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, double& parentSum)
|
||||
{
|
||||
parentSum += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
|
||||
}
|
||||
}
|
||||
|
||||
optimizeGradientSearchInPlace(bayesTree, grad);
|
||||
/* ************************************************************************* */
|
||||
VectorValuesUnordered GaussianBayesTreeUnordered::optimize() const {
|
||||
internal::OptimizeClique visitor;
|
||||
VectorValuesUnordered empty;
|
||||
treeTraversal::DepthFirstForest(*this, empty, visitor);
|
||||
return visitor.collectedResult;
|
||||
}
|
||||
|
||||
return grad;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
VectorValuesUnordered GaussianBayesTreeUnordered::optimizeGradientSearch() const
|
||||
{
|
||||
gttic(Compute_Gradient);
|
||||
// Compute gradient (call gradientAtZero function, which is defined for various linear systems)
|
||||
VectorValuesUnordered grad;
|
||||
bayesTree.gradientAtZeroInPlace(grad);
|
||||
double gradientSqNorm = grad.dot(grad);
|
||||
gttoc(Compute_Gradient);
|
||||
|
||||
/* ************************************************************************* */
|
||||
void optimizeGradientSearchInPlace(const GaussianBayesTree& bayesTree, VectorValues& grad) {
|
||||
gttic(Compute_Gradient);
|
||||
// Compute gradient (call gradientAtZero function, which is defined for various linear systems)
|
||||
gradientAtZero(bayesTree, grad);
|
||||
double gradientSqNorm = grad.dot(grad);
|
||||
gttoc(Compute_Gradient);
|
||||
gttic(Compute_Rg);
|
||||
// Compute R * g
|
||||
Errors Rg = GaussianFactorGraphUnordered(*this) * grad;
|
||||
gttoc(Compute_Rg);
|
||||
|
||||
gttic(Compute_Rg);
|
||||
// Compute R * g
|
||||
FactorGraph<JacobianFactor> Rd_jfg(bayesTree);
|
||||
Errors Rg = Rd_jfg * grad;
|
||||
gttoc(Compute_Rg);
|
||||
gttic(Compute_minimizing_step_size);
|
||||
// Compute minimizing step size
|
||||
double step = -gradientSqNorm / dot(Rg, Rg);
|
||||
gttoc(Compute_minimizing_step_size);
|
||||
|
||||
gttic(Compute_minimizing_step_size);
|
||||
// Compute minimizing step size
|
||||
double step = -gradientSqNorm / dot(Rg, Rg);
|
||||
gttoc(Compute_minimizing_step_size);
|
||||
gttic(Compute_point);
|
||||
// Compute steepest descent point
|
||||
scal(step, grad);
|
||||
gttoc(Compute_point);
|
||||
|
||||
gttic(Compute_point);
|
||||
// Compute steepest descent point
|
||||
scal(step, grad);
|
||||
gttoc(Compute_point);
|
||||
}
|
||||
return grad;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0) {
|
||||
return gradient(FactorGraph<JacobianFactor>(bayesTree), x0);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
VectorValuesUnordered GaussianBayesTreeUnordered::gradient(const VectorValuesUnordered& x0) const {
|
||||
return gtsam::gradient(GaussianFactorGraphUnordered(*this), x0);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g) {
|
||||
gradientAtZero(FactorGraph<JacobianFactor>(bayesTree), g);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void GaussianBayesTreeUnordered::gradientAtZeroInPlace(VectorValuesUnordered& g) const {
|
||||
gradientAtZero(GaussianFactorGraphUnordered(*this), g);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double determinant(const GaussianBayesTree& bayesTree) {
|
||||
if (!bayesTree.root())
|
||||
return 0.0;
|
||||
/* ************************************************************************* */
|
||||
double GaussianBayesTreeUnordered::logDeterminant() const
|
||||
{
|
||||
if(this->roots_.empty()) {
|
||||
return 0.0;
|
||||
} else {
|
||||
double sum = 0.0;
|
||||
treeTraversal::DepthFirstForest(*this, sum, internal::logDeterminant);
|
||||
return sum;
|
||||
}
|
||||
}
|
||||
|
||||
return exp(internal::logDeterminant<GaussianBayesTree>(bayesTree.root()));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
/* ************************************************************************* */
|
||||
double GaussianBayesTreeUnordered::determinant() const
|
||||
{
|
||||
return exp(logDeterminant());
|
||||
}
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -19,96 +19,112 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/inference/BayesTreeUnordered.h>
|
||||
#include <gtsam/inference/BayesTreeCliqueBaseUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// A Bayes Tree representing a Gaussian density
|
||||
GTSAM_EXPORT typedef BayesTree<GaussianConditional> GaussianBayesTree;
|
||||
// Forward declarations
|
||||
class GaussianFactorGraphUnordered;
|
||||
class GaussianBayesNetUnordered;
|
||||
class GaussianConditionalUnordered;
|
||||
class VectorValuesUnordered;
|
||||
|
||||
/// optimize the BayesTree, starting from the root
|
||||
GTSAM_EXPORT VectorValues optimize(const GaussianBayesTree& bayesTree);
|
||||
/* ************************************************************************* */
|
||||
/** A clique in a GaussianBayesTree */
|
||||
class GTSAM_EXPORT GaussianBayesTreeCliqueUnordered :
|
||||
public BayesTreeCliqueBaseUnordered<GaussianBayesTreeCliqueUnordered, GaussianFactorGraphUnordered, GaussianBayesNetUnordered>
|
||||
{
|
||||
public:
|
||||
typedef GaussianBayesTreeCliqueUnordered This;
|
||||
typedef BayesTreeCliqueBaseUnordered<GaussianBayesTreeCliqueUnordered, GaussianFactorGraphUnordered, GaussianBayesNetUnordered> Base;
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
typedef boost::weak_ptr<This> weak_ptr;
|
||||
GaussianBayesTreeCliqueUnordered() {}
|
||||
GaussianBayesTreeCliqueUnordered(const boost::shared_ptr<GaussianConditionalUnordered>& conditional) : Base(conditional) {}
|
||||
};
|
||||
|
||||
/// recursively optimize this conditional and all subtrees
|
||||
GTSAM_EXPORT void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result);
|
||||
/* ************************************************************************* */
|
||||
/** A Bayes tree representing a Gaussian density */
|
||||
class GTSAM_EXPORT GaussianBayesTreeUnordered :
|
||||
public BayesTreeUnordered<GaussianBayesTreeCliqueUnordered>
|
||||
{
|
||||
private:
|
||||
typedef BayesTreeUnordered<GaussianBayesTreeCliqueUnordered> Base;
|
||||
|
||||
namespace internal {
|
||||
template<class BAYESTREE>
|
||||
void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result);
|
||||
}
|
||||
public:
|
||||
typedef GaussianBayesTreeUnordered This;
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/**
|
||||
* Optimize along the gradient direction, with a closed-form computation to
|
||||
* perform the line search. The gradient is computed about \f$ \delta x=0 \f$.
|
||||
*
|
||||
* This function returns \f$ \delta x \f$ that minimizes a reparametrized
|
||||
* problem. The error function of a GaussianBayesNet is
|
||||
*
|
||||
* \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f]
|
||||
*
|
||||
* with gradient and Hessian
|
||||
*
|
||||
* \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f]
|
||||
*
|
||||
* This function performs the line search in the direction of the
|
||||
* gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size
|
||||
* \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$:
|
||||
*
|
||||
* \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f]
|
||||
*
|
||||
* Optimizing by setting the derivative to zero yields
|
||||
* \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function
|
||||
* evaluates the denominator without computing the Hessian \f$ G \f$, returning
|
||||
*
|
||||
* \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f]
|
||||
*/
|
||||
GTSAM_EXPORT VectorValues optimizeGradientSearch(const GaussianBayesTree& bayesTree);
|
||||
/** Check equality */
|
||||
bool equals(const This& other, double tol = 1e-9) const;
|
||||
|
||||
/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */
|
||||
GTSAM_EXPORT void optimizeGradientSearchInPlace(const GaussianBayesTree& bayesTree, VectorValues& grad);
|
||||
/** Recursively optimize the BayesTree to produce a vector solution. */
|
||||
VectorValuesUnordered optimize() const;
|
||||
|
||||
/**
|
||||
* Compute the gradient of the energy function,
|
||||
* \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$,
|
||||
* centered around \f$ x = x_0 \f$.
|
||||
* The gradient is \f$ R^T(Rx-d) \f$.
|
||||
* @param bayesTree The Gaussian Bayes Tree $(R,d)$
|
||||
* @param x0 The center about which to compute the gradient
|
||||
* @return The gradient as a VectorValues
|
||||
*/
|
||||
GTSAM_EXPORT VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0);
|
||||
/** Recursively optimize the BayesTree to produce a vector solution (same as optimize()), but
|
||||
* write the solution in place in \c result. */
|
||||
//void optimizeInPlace(VectorValuesUnordered& result) const;
|
||||
|
||||
/**
|
||||
* Compute the gradient of the energy function,
|
||||
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$,
|
||||
* centered around zero.
|
||||
* The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&).
|
||||
* @param bayesTree The Gaussian Bayes Tree $(R,d)$
|
||||
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
|
||||
* @return The gradient as a VectorValues
|
||||
*/
|
||||
GTSAM_EXPORT void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g);
|
||||
/**
|
||||
* Optimize along the gradient direction, with a closed-form computation to perform the line
|
||||
* search. The gradient is computed about \f$ \delta x=0 \f$.
|
||||
*
|
||||
* This function returns \f$ \delta x \f$ that minimizes a reparametrized problem. The error
|
||||
* function of a GaussianBayesNet is
|
||||
*
|
||||
* \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x +
|
||||
* \frac{1}{2} \delta x^T R^T R \delta x \f]
|
||||
*
|
||||
* with gradient and Hessian
|
||||
*
|
||||
* \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f]
|
||||
*
|
||||
* This function performs the line search in the direction of the gradient evaluated at \f$ g =
|
||||
* g(\delta x = 0) \f$ with step size \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g)
|
||||
* \f$:
|
||||
*
|
||||
* \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f]
|
||||
*
|
||||
* Optimizing by setting the derivative to zero yields \f$ \hat \alpha = (-g^T g) / (g^T G g)
|
||||
* \f$. For efficiency, this function evaluates the denominator without computing the Hessian
|
||||
* \f$ G \f$, returning
|
||||
*
|
||||
* \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] */
|
||||
VectorValuesUnordered optimizeGradientSearch() const;
|
||||
|
||||
/**
|
||||
* Computes the determinant of a GassianBayesTree
|
||||
* A GassianBayesTree is an upper triangular matrix and for an upper triangular matrix
|
||||
* determinant is the product of the diagonal elements. Instead of actually multiplying
|
||||
* we add the logarithms of the diagonal elements and take the exponent at the end
|
||||
* because this is more numerically stable.
|
||||
* @param bayesTree The input GassianBayesTree
|
||||
* @return The determinant
|
||||
*/
|
||||
GTSAM_EXPORT double determinant(const GaussianBayesTree& bayesTree);
|
||||
/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */
|
||||
void optimizeGradientSearchInPlace(VectorValuesUnordered& grad) const;
|
||||
|
||||
/** Compute the gradient of the energy function, \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x -
|
||||
* d \right\Vert^2 \f$, centered around \f$ x = x_0 \f$. The gradient is \f$ R^T(Rx-d) \f$.
|
||||
*
|
||||
* @param x0 The center about which to compute the gradient
|
||||
* @return The gradient as a VectorValues */
|
||||
VectorValuesUnordered gradient(const VectorValuesUnordered& x0) const;
|
||||
|
||||
namespace internal {
|
||||
template<class BAYESTREE>
|
||||
double logDeterminant(const typename BAYESTREE::sharedClique& clique);
|
||||
}
|
||||
/** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d
|
||||
* \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$. See also
|
||||
* gradient(const GaussianBayesNet&, const VectorValues&).
|
||||
*
|
||||
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see
|
||||
* allocateVectorValues */
|
||||
void gradientAtZeroInPlace(VectorValuesUnordered& g) const;
|
||||
|
||||
/** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a
|
||||
* matrix. A GassianBayesTree is equivalent to an upper triangular matrix, and for an upper
|
||||
* triangular matrix determinant is the product of the diagonal elements. Instead of actually
|
||||
* multiplying we add the logarithms of the diagonal elements and take the exponent at the end
|
||||
* because this is more numerically stable. */
|
||||
double determinant() const;
|
||||
|
||||
/** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a
|
||||
* matrix. A GassianBayesTree is equivalent to an upper triangular matrix, and for an upper
|
||||
* triangular matrix determinant is the product of the diagonal elements. Instead of actually
|
||||
* multiplying we add the logarithms of the diagonal elements and take the exponent at the end
|
||||
* because this is more numerically stable. */
|
||||
double logDeterminant() const;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#include <gtsam/linear/GaussianBayesTree-inl.h>
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue