Work on unordered GaussianConditional and GaussianBayesNet
Richard Roberts
parent
ed6a077e9e
commit
f6ad82eee6
|
|
@ -15,154 +15,81 @@
|
|||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/inference/BayesNet-inl.h>
|
||||
#include <gtsam/linear/GaussianBayesNetUnordered.h>
|
||||
#include <gtsam/linear/GaussianFactorGraphUnordered.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
|
||||
#include <stdarg.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// trick from some reading group
|
||||
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
|
||||
#define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL)
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Explicitly instantiate so we don't have to include everywhere
|
||||
template class BayesNet<GaussianConditional>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianBayesNet scalarGaussian(Index key, double mu, double sigma) {
|
||||
GaussianBayesNet bn;
|
||||
GaussianConditional::shared_ptr
|
||||
conditional(new GaussianConditional(key, Vector_(1,mu)/sigma, eye(1)/sigma, ones(1)));
|
||||
bn.push_back(conditional);
|
||||
return bn;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianBayesNet simpleGaussian(Index key, const Vector& mu, double sigma) {
|
||||
GaussianBayesNet bn;
|
||||
size_t n = mu.size();
|
||||
GaussianConditional::shared_ptr
|
||||
conditional(new GaussianConditional(key, mu/sigma, eye(n)/sigma, ones(n)));
|
||||
bn.push_back(conditional);
|
||||
return bn;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R,
|
||||
Index name1, Matrix S, Vector sigmas) {
|
||||
GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, sigmas));
|
||||
bn.push_front(cg);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R,
|
||||
Index name1, Matrix S, Index name2, Matrix T, Vector sigmas) {
|
||||
GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, name2, T, sigmas));
|
||||
bn.push_front(cg);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn) {
|
||||
vector<size_t> dimensions(bn.size());
|
||||
Index var = 0;
|
||||
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> conditional, bn) {
|
||||
dimensions[var++] = conditional->dim();
|
||||
/* ************************************************************************* */
|
||||
bool GaussianBayesNetUnordered::equals(const This& bn, double tol = 1e-9) const
|
||||
{
|
||||
return Base::equals(bn, tol);
|
||||
}
|
||||
return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues optimize(const GaussianBayesNet& bn) {
|
||||
VectorValues x = *allocateVectorValues(bn);
|
||||
optimizeInPlace(bn, x);
|
||||
return x;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
|
||||
void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x) {
|
||||
/* ************************************************************************* */
|
||||
VectorValuesUnordered GaussianBayesNetUnordered::optimize() const
|
||||
{
|
||||
VectorValuesUnordered soln;
|
||||
// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
|
||||
/** solve each node in turn in topological sort order (parents first)*/
|
||||
BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
|
||||
BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) {
|
||||
// i^th part of R*x=y, x=inv(R)*y
|
||||
// (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
|
||||
cg->solveInPlace(x);
|
||||
soln.insert(cg->solve(soln));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& input) {
|
||||
VectorValues output = input;
|
||||
BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
|
||||
const Index key = *(cg->beginFrontals());
|
||||
Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents());
|
||||
xS = input[key] - cg->get_S() * xS;
|
||||
output[key] = cg->get_R().triangularView<Eigen::Upper>().solve(xS);
|
||||
return soln;
|
||||
}
|
||||
|
||||
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
|
||||
cg->scaleFrontalsBySigma(output);
|
||||
/* ************************************************************************* */
|
||||
VectorValuesUnordered GaussianBayesNetUnordered::backSubstitute(const VectorValuesUnordered& rhs) const
|
||||
{
|
||||
VectorValuesUnordered result;
|
||||
BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) {
|
||||
result.insert(cg->solveOtherRHS(result, rhs));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
// gy=inv(L)*gx by solving L*gy=gx.
|
||||
// gy=inv(R'*inv(Sigma))*gx
|
||||
// gz'*R'=gx', gy = gz.*sigmas
|
||||
VectorValues backSubstituteTranspose(const GaussianBayesNet& bn,
|
||||
const VectorValues& gx) {
|
||||
|
||||
/* ************************************************************************* */
|
||||
// gy=inv(L)*gx by solving L*gy=gx.
|
||||
// gy=inv(R'*inv(Sigma))*gx
|
||||
// gz'*R'=gx', gy = gz.*sigmas
|
||||
VectorValuesUnordered GaussianBayesNetUnordered::backSubstituteTranspose(const VectorValuesUnordered& gx) const
|
||||
{
|
||||
// Initialize gy from gx
|
||||
// TODO: used to insert zeros if gx did not have an entry for a variable in bn
|
||||
VectorValues gy = gx;
|
||||
VectorValuesUnordered gy = gx;
|
||||
|
||||
// we loop from first-eliminated to last-eliminated
|
||||
// i^th part of L*gy=gx is done block-column by block-column of L
|
||||
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn)
|
||||
BOOST_FOREACH(const sharedConditional& cg, *this)
|
||||
cg->solveTransposeInPlace(gy);
|
||||
|
||||
// Scale gy
|
||||
BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn)
|
||||
cg->scaleFrontalsBySigma(gy);
|
||||
|
||||
return gy;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues optimizeGradientSearch(const GaussianBayesNet& Rd) {
|
||||
gttic(Allocate_VectorValues);
|
||||
VectorValues grad = *allocateVectorValues(Rd);
|
||||
gttoc(Allocate_VectorValues);
|
||||
|
||||
optimizeGradientSearchInPlace(Rd, grad);
|
||||
|
||||
return grad;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void optimizeGradientSearchInPlace(const GaussianBayesNet& Rd, VectorValues& grad) {
|
||||
/* ************************************************************************* */
|
||||
VectorValuesUnordered GaussianBayesNetUnordered::optimizeGradientSearch() const
|
||||
{
|
||||
gttic(Compute_Gradient);
|
||||
// Compute gradient (call gradientAtZero function, which is defined for various linear systems)
|
||||
gradientAtZero(Rd, grad);
|
||||
VectorValuesUnordered grad = gradientAtZero();
|
||||
double gradientSqNorm = grad.dot(grad);
|
||||
gttoc(Compute_Gradient);
|
||||
|
||||
gttic(Compute_Rg);
|
||||
// Compute R * g
|
||||
FactorGraph<JacobianFactor> Rd_jfg(Rd);
|
||||
Errors Rg = Rd_jfg * grad;
|
||||
Errors Rg = GaussianFactorGraphUnordered(*this) * grad;
|
||||
gttoc(Compute_Rg);
|
||||
|
||||
gttic(Compute_minimizing_step_size);
|
||||
|
|
@ -174,63 +101,18 @@ void optimizeGradientSearchInPlace(const GaussianBayesNet& Rd, VectorValues& gra
|
|||
// Compute steepest descent point
|
||||
scal(step, grad);
|
||||
gttoc(Compute_point);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
|
||||
|
||||
// add the dimensions of all variables to get matrix dimension
|
||||
// and at the same time create a mapping from keys to indices
|
||||
size_t N=0; map<Index,size_t> mapping;
|
||||
BOOST_FOREACH(GaussianConditional::shared_ptr cg,bn) {
|
||||
GaussianConditional::const_iterator it = cg->beginFrontals();
|
||||
for (; it != cg->endFrontals(); ++it) {
|
||||
mapping.insert(make_pair(*it,N));
|
||||
N += cg->dim(it);
|
||||
}
|
||||
return grad;
|
||||
}
|
||||
|
||||
// create matrix and copy in values
|
||||
Matrix R = zeros(N,N);
|
||||
Vector d(N);
|
||||
Index key; size_t I;
|
||||
FOREACH_PAIR(key,I,mapping) {
|
||||
// find corresponding conditional
|
||||
boost::shared_ptr<const GaussianConditional> cg = bn[key];
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix,Vector> GaussianBayesNetUnordered::matrix() const
|
||||
{
|
||||
return GaussianFactorGraphUnordered(*this).jacobian();
|
||||
}
|
||||
|
||||
// get sigmas
|
||||
Vector sigmas = cg->get_sigmas();
|
||||
|
||||
// get RHS and copy to d
|
||||
GaussianConditional::const_d_type d_ = cg->get_d();
|
||||
const size_t n = d_.size();
|
||||
for (size_t i=0;i<n;i++)
|
||||
d(I+i) = d_(i)/sigmas(i);
|
||||
|
||||
// get leading R matrix and copy to R
|
||||
GaussianConditional::const_r_type R_ = cg->get_R();
|
||||
for (size_t i=0;i<n;i++)
|
||||
for(size_t j=0;j<n;j++)
|
||||
R(I+i,I+j) = R_(i,j)/sigmas(i);
|
||||
|
||||
// loop over S matrices and copy them into R
|
||||
GaussianConditional::const_iterator keyS = cg->beginParents();
|
||||
for (; keyS!=cg->endParents(); keyS++) {
|
||||
Matrix S = cg->get_S(keyS); // get S matrix
|
||||
const size_t m = S.rows(), n = S.cols(); // find S size
|
||||
const size_t J = mapping[*keyS]; // find column index
|
||||
for (size_t i=0;i<m;i++)
|
||||
for(size_t j=0;j<n;j++)
|
||||
R(I+i,J+j) = S(i,j)/sigmas(i);
|
||||
} // keyS
|
||||
|
||||
} // keyI
|
||||
|
||||
return make_pair(R,d);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double determinant(const GaussianBayesNet& bayesNet) {
|
||||
/* ************************************************************************* */
|
||||
double determinant(const GaussianBayesNet& bayesNet) {
|
||||
double logDet = 0.0;
|
||||
|
||||
BOOST_FOREACH(boost::shared_ptr<const GaussianConditional> cg, bayesNet){
|
||||
|
|
@ -238,18 +120,18 @@ double determinant(const GaussianBayesNet& bayesNet) {
|
|||
}
|
||||
|
||||
return exp(logDet);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0) {
|
||||
/* ************************************************************************* */
|
||||
VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0) {
|
||||
return gradient(GaussianFactorGraph(bayesNet), x0);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g) {
|
||||
/* ************************************************************************* */
|
||||
void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g) {
|
||||
gradientAtZero(GaussianFactorGraph(bayesNet), g);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -20,54 +20,50 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/GaussianConditionalUnordered.h>
|
||||
#include <gtsam/inference/BayesNetUnordered.h>
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/inference/BayesNet.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** A Bayes net made from linear-Gaussian densities */
|
||||
typedef BayesNet<GaussianConditional> GaussianBayesNet;
|
||||
class GTSAM_EXPORT GaussianBayesNetUnordered: public BayesNetUnordered<GaussianConditionalUnordered> {
|
||||
|
||||
/** Create a scalar Gaussian */
|
||||
GTSAM_EXPORT GaussianBayesNet scalarGaussian(Index key, double mu=0.0, double sigma=1.0);
|
||||
public:
|
||||
|
||||
/** Create a simple Gaussian on a single multivariate variable */
|
||||
GTSAM_EXPORT GaussianBayesNet simpleGaussian(Index key, const Vector& mu, double sigma=1.0);
|
||||
typedef BayesNetUnordered<GaussianConditionalUnordered> Base;
|
||||
typedef GaussianBayesNetUnordered This;
|
||||
typedef GaussianConditionalUnordered ConditionalType;
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/**
|
||||
* Add a conditional node with one parent
|
||||
* |Rx+Sy-d|
|
||||
*/
|
||||
GTSAM_EXPORT void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R,
|
||||
Index name1, Matrix S, Vector sigmas);
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Add a conditional node with two parents
|
||||
* |Rx+Sy+Tz-d|
|
||||
*/
|
||||
GTSAM_EXPORT void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R,
|
||||
Index name1, Matrix S, Index name2, Matrix T, Vector sigmas);
|
||||
/** Construct empty factor graph */
|
||||
GaussianBayesNetUnordered() {}
|
||||
|
||||
/**
|
||||
* Allocate a VectorValues for the variables in a BayesNet
|
||||
*/
|
||||
GTSAM_EXPORT boost::shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn);
|
||||
/** Construct from iterator over conditionals */
|
||||
template<typename ITERATOR>
|
||||
GaussianBayesNetUnordered(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {}
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** Check equality */
|
||||
bool equals(const This& bn, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by
|
||||
* back-substitution.
|
||||
*/
|
||||
GTSAM_EXPORT VectorValues optimize(const GaussianBayesNet& bn);
|
||||
|
||||
/**
|
||||
* Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by
|
||||
* back-substitution, writes the solution \f$ x \f$ into a pre-allocated
|
||||
* VectorValues. You can use allocateVectorValues(const GaussianBayesNet&)
|
||||
* allocate it. See also optimize(const GaussianBayesNet&), which does not
|
||||
* require pre-allocation.
|
||||
*/
|
||||
GTSAM_EXPORT void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x);
|
||||
VectorValuesUnordered optimize() const;
|
||||
|
||||
/**
|
||||
* Optimize along the gradient direction, with a closed-form computation to
|
||||
|
|
@ -97,44 +93,17 @@ namespace gtsam {
|
|||
* @param bn The GaussianBayesNet on which to perform this computation
|
||||
* @return The resulting \f$ \delta x \f$ as described above
|
||||
*/
|
||||
GTSAM_EXPORT VectorValues optimizeGradientSearch(const GaussianBayesNet& bn);
|
||||
VectorValuesUnordered optimizeGradientSearch() const;
|
||||
|
||||
/** In-place version of optimizeGradientSearch(const GaussianBayesNet&) requiring pre-allocated VectorValues \c grad
|
||||
*
|
||||
* @param bn The GaussianBayesNet on which to perform this computation
|
||||
* @param [out] grad The resulting \f$ \delta x \f$ as described in optimizeGradientSearch(const GaussianBayesNet&)
|
||||
* */
|
||||
GTSAM_EXPORT void optimizeGradientSearchInPlace(const GaussianBayesNet& bn, VectorValues& grad);
|
||||
///@}
|
||||
|
||||
/**
|
||||
* Backsubstitute
|
||||
* gy=inv(R*inv(Sigma))*gx
|
||||
*/
|
||||
GTSAM_EXPORT VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& gx);
|
||||
|
||||
/**
|
||||
* Transpose Backsubstitute
|
||||
* gy=inv(L)*gx by solving L*gy=gx.
|
||||
* gy=inv(R'*inv(Sigma))*gx
|
||||
* gz'*R'=gx', gy = gz.*sigmas
|
||||
*/
|
||||
GTSAM_EXPORT VectorValues backSubstituteTranspose(const GaussianBayesNet& bn, const VectorValues& gx);
|
||||
///@name Linear Algebra
|
||||
///@{
|
||||
|
||||
/**
|
||||
* Return (dense) upper-triangular matrix representation
|
||||
*/
|
||||
GTSAM_EXPORT std::pair<Matrix, Vector> matrix(const GaussianBayesNet&);
|
||||
|
||||
/**
|
||||
* Computes the determinant of a GassianBayesNet
|
||||
* A GaussianBayesNet 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 bayesNet The input GaussianBayesNet
|
||||
* @return The determinant
|
||||
*/
|
||||
GTSAM_EXPORT double determinant(const GaussianBayesNet& bayesNet);
|
||||
std::pair<Matrix, Vector> matrix() const;
|
||||
|
||||
/**
|
||||
* Compute the gradient of the energy function,
|
||||
|
|
@ -145,7 +114,7 @@ namespace gtsam {
|
|||
* @param x0 The center about which to compute the gradient
|
||||
* @return The gradient as a VectorValues
|
||||
*/
|
||||
GTSAM_EXPORT VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0);
|
||||
VectorValuesUnordered gradient(const VectorValuesUnordered& x0) const;
|
||||
|
||||
/**
|
||||
* Compute the gradient of the energy function,
|
||||
|
|
@ -156,6 +125,34 @@ namespace gtsam {
|
|||
* @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 GaussianBayesNet& bayesNet, VectorValues& g);
|
||||
VectorValuesUnordered gradientAtZero() const;
|
||||
|
||||
/**
|
||||
* Computes the determinant of a GassianBayesNet
|
||||
* A GaussianBayesNet 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 bayesNet The input GaussianBayesNet
|
||||
* @return The determinant
|
||||
*/
|
||||
double determinant() const;
|
||||
|
||||
/**
|
||||
* Backsubstitute with a different RHS vector than the one stored in this BayesNet.
|
||||
* gy=inv(R*inv(Sigma))*gx
|
||||
*/
|
||||
VectorValuesUnordered backSubstitute(const VectorValuesUnordered& gx) const;
|
||||
|
||||
/**
|
||||
* Transpose backsubstitute with a different RHS vector than the one stored in this BayesNet.
|
||||
* gy=inv(L)*gx by solving L*gy=gx.
|
||||
* gy=inv(R'*inv(Sigma))*gx
|
||||
* gz'*R'=gx', gy = gz.*sigmas
|
||||
*/
|
||||
VectorValuesUnordered backSubstituteTranspose(const VectorValuesUnordered& gx) const;
|
||||
|
||||
/// @}
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -72,8 +72,7 @@ namespace gtsam {
|
|||
{
|
||||
gttic(Compute_Gradient);
|
||||
// Compute gradient (call gradientAtZero function, which is defined for various linear systems)
|
||||
VectorValuesUnordered grad;
|
||||
bayesTree.gradientAtZeroInPlace(grad);
|
||||
VectorValuesUnordered grad = gradientAtZero();
|
||||
double gradientSqNorm = grad.dot(grad);
|
||||
gttoc(Compute_Gradient);
|
||||
|
||||
|
|
|
|||
|
|
@ -104,47 +104,83 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianConditionalUnordered::solveInPlace(VectorValuesUnordered& x) const {
|
||||
static const bool debug = false;
|
||||
if(debug) this->print("Solving conditional in place");
|
||||
Vector xS = internal::extractVectorValuesSlices(x, this->beginParents(), this->endParents());
|
||||
xS = this->getb() - this->get_S() * xS;
|
||||
Vector soln = this->get_R().triangularView<Eigen::Upper>().solve(xS);
|
||||
VectorValuesUnordered GaussianConditionalUnordered::solve(const VectorValuesUnordered& x) const
|
||||
{
|
||||
// Solve matrix
|
||||
Vector xS = x.vector(vector<Key>(beginParents(), endParents()));
|
||||
xS = getb() - get_S() * xS;
|
||||
Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);
|
||||
|
||||
// Check for indeterminant solution
|
||||
if(soln.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite<double>), boost::lambda::_1)).any())
|
||||
throw IndeterminantLinearSystemException(this->keys().front());
|
||||
throw IndeterminantLinearSystemException(keys().front());
|
||||
|
||||
if(debug) {
|
||||
gtsam::print(Matrix(this->get_R()), "Calling backSubstituteUpper on ");
|
||||
gtsam::print(soln, "full back-substitution solution: ");
|
||||
// Insert solution into a VectorValues
|
||||
VectorValuesUnordered result;
|
||||
DenseIndex vectorPosition = 0;
|
||||
for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
|
||||
result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal)));
|
||||
vectorPosition += getDim(frontal);
|
||||
}
|
||||
internal::writeVectorValuesSlices(soln, x, this->beginFrontals(), this->endFrontals());
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianConditionalUnordered::solveTransposeInPlace(VectorValuesUnordered& gy) const {
|
||||
Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
|
||||
frontalVec = gtsam::backSubstituteUpper(frontalVec,Matrix(get_R()));
|
||||
VectorValuesUnordered GaussianConditionalUnordered::solveOtherRHS(
|
||||
const VectorValuesUnordered& parents, const VectorValuesUnordered& rhs) const
|
||||
{
|
||||
Vector xS = parents.vector(vector<Key>(beginParents(), endParents()));
|
||||
const Vector rhsR = rhs.vector(vector<Key>(beginFrontals(), endFrontals()));
|
||||
xS = rhsR - get_S() * xS;
|
||||
Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);
|
||||
|
||||
// Scale by sigmas
|
||||
soln.array() *= model_->sigmas().array();
|
||||
|
||||
// Insert solution into a VectorValues
|
||||
VectorValuesUnordered result;
|
||||
DenseIndex vectorPosition = 0;
|
||||
for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
|
||||
result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal)));
|
||||
vectorPosition += getDim(frontal);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianConditionalUnordered::solveTransposeInPlace(VectorValuesUnordered& gy) const
|
||||
{
|
||||
Vector frontalVec = gy.vector(vector<Key>(beginFrontals(), endFrontals()));
|
||||
frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R()));
|
||||
|
||||
// Check for indeterminant solution
|
||||
if(frontalVec.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite<double>), boost::lambda::_1)).any())
|
||||
throw IndeterminantLinearSystemException(this->keys().front());
|
||||
|
||||
GaussianConditionalUnordered::const_iterator it;
|
||||
for (it = beginParents(); it!= endParents(); it++) {
|
||||
const Key i = *it;
|
||||
gtsam::transposeMultiplyAdd(-1.0, getA(it), frontalVec, gy[i]);
|
||||
for (const_iterator it = beginParents(); it!= endParents(); it++)
|
||||
gtsam::transposeMultiplyAdd(-1.0, Matrix(getA(it)), frontalVec, gy[*it]);
|
||||
|
||||
// Scale by sigmas
|
||||
frontalVec.array() *= model_->sigmas().array();
|
||||
|
||||
// Write frontal solution into a VectorValues
|
||||
DenseIndex vectorPosition = 0;
|
||||
for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
|
||||
gy[*frontal] = frontalVec.segment(vectorPosition, getDim(frontal));
|
||||
vectorPosition += getDim(frontal);
|
||||
}
|
||||
internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianConditionalUnordered::scaleFrontalsBySigma(VectorValuesUnordered& gy) const {
|
||||
Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
|
||||
if(model_)
|
||||
frontalVec.array() *= model_->sigmas().array();
|
||||
internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
|
||||
void GaussianConditionalUnordered::scaleFrontalsBySigma(VectorValuesUnordered& gy) const
|
||||
{
|
||||
DenseIndex vectorPosition = 0;
|
||||
for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
|
||||
gy[*frontal].array() *= model_->sigmas().segment(vectorPosition, getDim(frontal)).array();
|
||||
vectorPosition += getDim(frontal);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -117,17 +117,17 @@ namespace gtsam {
|
|||
* variables of this conditional, this solve function computes
|
||||
* \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution.
|
||||
*
|
||||
* @param x VectorValues structure with solved parents \f$ x_s \f$, and into which the
|
||||
* solution \f$ x_f \f$ will be written.
|
||||
* @param parents VectorValues containing solved parents \f$ x_s \f$.
|
||||
*/
|
||||
void solveInPlace(VectorValuesUnordered& x) const;
|
||||
VectorValuesUnordered solve(const VectorValuesUnordered& parents) const;
|
||||
|
||||
// functions for transpose backsubstitution
|
||||
VectorValuesUnordered solveOtherRHS(const VectorValuesUnordered& parents, const VectorValuesUnordered& rhs) const;
|
||||
|
||||
/**
|
||||
* Performs backsubstition in place on values
|
||||
*/
|
||||
/** Performs transpose backsubstition in place on values */
|
||||
void solveTransposeInPlace(VectorValuesUnordered& gy) const;
|
||||
|
||||
/** Scale the values in \c gy according to the sigmas for the frontal variables in this
|
||||
* conditional. */
|
||||
void scaleFrontalsBySigma(VectorValuesUnordered& gy) const;
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -139,13 +139,8 @@ namespace gtsam {
|
|||
return exp(-0.5 * error(c));
|
||||
}
|
||||
|
||||
/**
|
||||
* Static function that combines two factor graphs.
|
||||
* @param lfg1 Linear factor graph
|
||||
* @param lfg2 Linear factor graph
|
||||
* @return a new combined factor graph
|
||||
*/
|
||||
static This combine2(const This& lfg1, const This& lfg2);
|
||||
///@name Linear Algebra
|
||||
///@{
|
||||
|
||||
/**
|
||||
* Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix,
|
||||
|
|
@ -200,6 +195,30 @@ namespace gtsam {
|
|||
*/
|
||||
//std::pair<Matrix,Vector> hessian() const;
|
||||
|
||||
/**
|
||||
* Compute the gradient of the energy function,
|
||||
* \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
|
||||
* centered around \f$ x = x_0 \f$.
|
||||
* The gradient is \f$ A^T(Ax-b) \f$.
|
||||
* @param fg The Jacobian factor graph $(A,b)$
|
||||
* @param x0 The center about which to compute the gradient
|
||||
* @return The gradient as a VectorValuesUnordered
|
||||
*/
|
||||
VectorValuesUnordered gradient(const VectorValuesUnordered& x0) const;
|
||||
|
||||
/**
|
||||
* Compute the gradient of the energy function,
|
||||
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
|
||||
* centered around zero.
|
||||
* The gradient is \f$ A^T(Ax-b) \f$.
|
||||
* @param fg The Jacobian factor graph $(A,b)$
|
||||
* @param [output] g A VectorValuesUnordered to store the gradient, which must be preallocated, see allocateVectorValues
|
||||
* @return The gradient as a VectorValuesUnordered
|
||||
*/
|
||||
VectorValuesUnordered gradientAtZero() const;
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
@ -273,30 +292,6 @@ namespace gtsam {
|
|||
/** x += alpha*A'*e */
|
||||
GTSAM_EXPORT void transposeMultiplyAdd(const GaussianFactorGraphUnordered& fg, double alpha, const Errors& e, VectorValuesUnordered& x);
|
||||
|
||||
/**
|
||||
* Compute the gradient of the energy function,
|
||||
* \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
|
||||
* centered around \f$ x = x_0 \f$.
|
||||
* The gradient is \f$ A^T(Ax-b) \f$.
|
||||
* @param fg The Jacobian factor graph $(A,b)$
|
||||
* @param x0 The center about which to compute the gradient
|
||||
* @return The gradient as a VectorValuesUnordered
|
||||
|
||||
*/
|
||||
GTSAM_EXPORT VectorValuesUnordered gradient(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x0);
|
||||
|
||||
/**
|
||||
* Compute the gradient of the energy function,
|
||||
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
|
||||
* centered around zero.
|
||||
* The gradient is \f$ A^T(Ax-b) \f$.
|
||||
* @param fg The Jacobian factor graph $(A,b)$
|
||||
* @param [output] g A VectorValuesUnordered to store the gradient, which must be preallocated, see allocateVectorValues
|
||||
* @return The gradient as a VectorValuesUnordered
|
||||
|
||||
*/
|
||||
GTSAM_EXPORT void gradientAtZero(const GaussianFactorGraphUnordered& fg, VectorValuesUnordered& g);
|
||||
|
||||
/* matrix-vector operations */
|
||||
GTSAM_EXPORT void residual(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r);
|
||||
GTSAM_EXPORT void multiply(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r);
|
||||
|
|
|
|||
Loading…
Reference in New Issue