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

96
gtsam/linear/GaussianBayesTreeUnordered.cpp
View File

@ -17,46 +17,69 @@
* @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()(
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;
}
};
/* ************************************************************************* */
double logDeterminant(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, double& parentSum)
{
parentSum += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) { VectorValuesUnordered GaussianBayesTreeUnordered::optimize() const {
internal::optimizeInPlace<GaussianBayesTree>(bayesTree.root(), result); internal::OptimizeClique visitor;
VectorValuesUnordered empty;
treeTraversal::DepthFirstForest(*this, empty, visitor);
return visitor.collectedResult;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues optimizeGradientSearch(const GaussianBayesTree& bayesTree) { VectorValuesUnordered GaussianBayesTreeUnordered::optimizeGradientSearch() const
gttic(Allocate_VectorValues); {
VectorValues grad = *allocateVectorValues(bayesTree);
gttoc(Allocate_VectorValues);
optimizeGradientSearchInPlace(bayesTree, grad);
return grad;
}
/* ************************************************************************* */
void optimizeGradientSearchInPlace(const GaussianBayesTree& bayesTree, VectorValues& grad) {
gttic(Compute_Gradient); gttic(Compute_Gradient);
// Compute gradient (call gradientAtZero function, which is defined for various linear systems) // Compute gradient (call gradientAtZero function, which is defined for various linear systems)
gradientAtZero(bayesTree, grad); VectorValuesUnordered grad;
bayesTree.gradientAtZeroInPlace(grad);
double gradientSqNorm = grad.dot(grad); double gradientSqNorm = grad.dot(grad);
gttoc(Compute_Gradient); gttoc(Compute_Gradient);
gttic(Compute_Rg); gttic(Compute_Rg);
// Compute R * g // Compute R * g
FactorGraph<JacobianFactor> Rd_jfg(bayesTree); Errors Rg = GaussianFactorGraphUnordered(*this) * grad;
Errors Rg = Rd_jfg * grad;
gttoc(Compute_Rg); gttoc(Compute_Rg);
gttic(Compute_minimizing_step_size); gttic(Compute_minimizing_step_size);
@ -68,26 +91,37 @@ void optimizeGradientSearchInPlace(const GaussianBayesTree& bayesTree, VectorVal
// Compute steepest descent point // Compute steepest descent point
scal(step, grad); scal(step, grad);
gttoc(Compute_point); gttoc(Compute_point);
return grad;
} }
/* ************************************************************************* */ /* ************************************************************************* */
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()) {
if(this->roots_.empty()) {
return 0.0; return 0.0;
} else {
return exp(internal::logDeterminant<GaussianBayesTree>(bayesTree.root())); double sum = 0.0;
treeTraversal::DepthFirstForest(*this, sum, internal::logDeterminant);
return sum;
} }
}
/* ************************************************************************* */ /* ************************************************************************* */
double GaussianBayesTreeUnordered::determinant() const
{
return exp(logDeterminant());
}
} // \namespace gtsam } // \namespace gtsam

144
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 */
bool equals(const This& other, double tol = 1e-9) const;
/** Recursively optimize the BayesTree to produce a vector solution. */
VectorValuesUnordered optimize() const;
/** 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;
/** /**
* Optimize along the gradient direction, with a closed-form computation to * Optimize along the gradient direction, with a closed-form computation to perform the line
* perform the line search. The gradient is computed about \f$ \delta x=0 \f$. * search. The gradient is computed about \f$ \delta x=0 \f$.
* *
* This function returns \f$ \delta x \f$ that minimizes a reparametrized * This function returns \f$ \delta x \f$ that minimizes a reparametrized problem. The error
* problem. The error function of a GaussianBayesNet is * 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] * \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 * with gradient and Hessian
* *
* \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] * \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 * This function performs the line search in the direction of the gradient evaluated at \f$ g =
* gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size * g(\delta x = 0) \f$ with step size \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g)
* \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: * \f$:
* *
* \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G 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 * Optimizing by setting the derivative to zero yields \f$ \hat \alpha = (-g^T g) / (g^T G g)
* \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function * \f$. For efficiency, this function evaluates the denominator without computing the Hessian
* evaluates the denominator without computing the Hessian \f$ G \f$, returning * \f$ G \f$, returning
* *
* \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] */
*/ VectorValuesUnordered optimizeGradientSearch() const;
GTSAM_EXPORT VectorValues optimizeGradientSearch(const GaussianBayesTree& bayesTree);
/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ /** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */
GTSAM_EXPORT void optimizeGradientSearchInPlace(const GaussianBayesTree& bayesTree, VectorValues& grad); void optimizeGradientSearchInPlace(VectorValuesUnordered& grad) const;
/** /** Compute the gradient of the energy function, \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x -
* Compute the gradient of the energy function, * d \right\Vert^2 \f$, centered around \f$ x = x_0 \f$. The gradient is \f$ R^T(Rx-d) \f$.
* \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 * @param x0 The center about which to compute the gradient
* @return The gradient as a VectorValues * @return The gradient as a VectorValues */
*/ VectorValuesUnordered gradient(const VectorValuesUnordered& x0) const;
GTSAM_EXPORT VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0);
/** /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d
* Compute the gradient of the energy function, * \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$. See also
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, * gradient(const GaussianBayesNet&, const VectorValues&).
* 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
* @param bayesTree The Gaussian Bayes Tree $(R,d)$ * allocateVectorValues */
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues void gradientAtZeroInPlace(VectorValuesUnordered& g) const;
* @return The gradient as a VectorValues
*/
GTSAM_EXPORT void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g);
/** /** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a
* Computes the determinant of a GassianBayesTree * matrix. A GassianBayesTree is equivalent to an upper triangular matrix, and for an upper
* A GassianBayesTree is an upper triangular matrix and for an upper triangular matrix * triangular matrix determinant is the product of the diagonal elements. Instead of actually
* determinant is the product of the diagonal elements. Instead of actually multiplying * multiplying we add the logarithms of the diagonal elements and take the exponent at the end
* we add the logarithms of the diagonal elements and take the exponent at the end * because this is more numerically stable. */
* because this is more numerically stable. double determinant() const;
* @param bayesTree The input GassianBayesTree
* @return The determinant
*/
GTSAM_EXPORT double determinant(const GaussianBayesTree& bayesTree);
/** 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;
namespace internal { };
template<class BAYESTREE>
double logDeterminant(const typename BAYESTREE::sharedClique& clique);
}
} }
#include <gtsam/linear/GaussianBayesTree-inl.h>