12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added unordered GaussianBayesTree

release/4.3a0
Richard Roberts 2013-07-02 14:54:22 +00:00
parent 79ce96dceb
commit 57f682998c
2 changed files with 184 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

146
gtsam/linear/GaussianBayesTreeUnordered.cpp
View File

@ -17,77 +17,111 @@
* @author Richard Roberts * @author Richard Roberts
*/ */
#include <gtsam/linear/GaussianBayesTree.h> #include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/linear/GaussianFactorGraph.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 { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues optimize(const GaussianBayesTree& bayesTree) { namespace internal
VectorValues result = *allocateVectorValues(bayesTree); {
optimizeInPlace(bayesTree, result); /* ************************************************************************* */
return result; /** 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;
/* ************************************************************************* */ VectorValuesUnordered operator()(
void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) { const GaussianBayesTreeCliqueUnordered::shared_ptr& clique,
internal::optimizeInPlace<GaussianBayesTree>(bayesTree.root(), result); 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) { double logDeterminant(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, double& parentSum)
gttic(Allocate_VectorValues); {
VectorValues grad = *allocateVectorValues(bayesTree); parentSum += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
gttoc(Allocate_VectorValues); }
}
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);
/* ************************************************************************* */ gttic(Compute_Rg);
void optimizeGradientSearchInPlace(const GaussianBayesTree& bayesTree, VectorValues& grad) { // Compute R * g
gttic(Compute_Gradient); Errors Rg = GaussianFactorGraphUnordered(*this) * grad;
// Compute gradient (call gradientAtZero function, which is defined for various linear systems) gttoc(Compute_Rg);
gradientAtZero(bayesTree, grad);
double gradientSqNorm = grad.dot(grad);
gttoc(Compute_Gradient);
gttic(Compute_Rg); gttic(Compute_minimizing_step_size);
// Compute R * g // Compute minimizing step size
FactorGraph<JacobianFactor> Rd_jfg(bayesTree); double step = -gradientSqNorm / dot(Rg, Rg);
Errors Rg = Rd_jfg * grad; gttoc(Compute_minimizing_step_size);
gttoc(Compute_Rg);
gttic(Compute_minimizing_step_size); gttic(Compute_point);
// Compute minimizing step size // Compute steepest descent point
double step = -gradientSqNorm / dot(Rg, Rg); scal(step, grad);
gttoc(Compute_minimizing_step_size); gttoc(Compute_point);
gttic(Compute_point); return grad;
// Compute steepest descent point }
scal(step, grad);
gttoc(Compute_point);
}
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0) { VectorValuesUnordered GaussianBayesTreeUnordered::gradient(const VectorValuesUnordered& x0) const {
return gradient(FactorGraph<JacobianFactor>(bayesTree), x0); return gtsam::gradient(GaussianFactorGraphUnordered(*this), x0);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g) { void GaussianBayesTreeUnordered::gradientAtZeroInPlace(VectorValuesUnordered& g) const {
gradientAtZero(FactorGraph<JacobianFactor>(bayesTree), g); gradientAtZero(GaussianFactorGraphUnordered(*this), g);
} }
/* ************************************************************************* */ /* ************************************************************************* */
double determinant(const GaussianBayesTree& bayesTree) { double GaussianBayesTreeUnordered::logDeterminant() const
if (!bayesTree.root()) {
return 0.0; 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 } // \namespace gtsam

172
gtsam/linear/GaussianBayesTreeUnordered.h
View File

@ -19,96 +19,112 @@
#pragma once #pragma once
#include <gtsam/inference/BayesTree.h> #include <gtsam/inference/BayesTreeUnordered.h>
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/inference/BayesTreeCliqueBaseUnordered.h>
#include <gtsam/linear/GaussianFactor.h>
namespace gtsam { namespace gtsam {
/// A Bayes Tree representing a Gaussian density // Forward declarations
GTSAM_EXPORT typedef BayesTree<GaussianConditional> GaussianBayesTree; 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 { public:
template<class BAYESTREE> typedef GaussianBayesTreeUnordered This;
void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result); typedef boost::shared_ptr<This> shared_ptr;
}
/** /** Check equality */
* Optimize along the gradient direction, with a closed-form computation to bool equals(const This& other, double tol = 1e-9) const;
* 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);
/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ /** Recursively optimize the BayesTree to produce a vector solution. */
GTSAM_EXPORT void optimizeGradientSearchInPlace(const GaussianBayesTree& bayesTree, VectorValues& grad); VectorValuesUnordered optimize() const;
/** /** Recursively optimize the BayesTree to produce a vector solution (same as optimize()), but
* Compute the gradient of the energy function, * write the solution in place in \c result. */
* \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, //void optimizeInPlace(VectorValuesUnordered& result) const;
* 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);
/** /**
* Compute the gradient of the energy function, * Optimize along the gradient direction, with a closed-form computation to perform the line
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, * search. The gradient is computed about \f$ \delta x=0 \f$.
* centered around zero. *
* The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). * This function returns \f$ \delta x \f$ that minimizes a reparametrized problem. The error
* @param bayesTree The Gaussian Bayes Tree $(R,d)$ * function of a GaussianBayesNet is
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues *
* @return The gradient as a VectorValues * \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]
GTSAM_EXPORT void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g); *
* 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;
/** /** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */
* Computes the determinant of a GassianBayesTree void optimizeGradientSearchInPlace(VectorValuesUnordered& grad) const;
* 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);
/** 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 { /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d
template<class BAYESTREE> * \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$. See also
double logDeterminant(const typename BAYESTREE::sharedClique& clique); * 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>