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

Work on unordered GaussianConditional and GaussianBayesNet

release/4.3a0
Richard Roberts 2013-07-05 20:45:58 +00:00
parent ed6a077e9e
commit f6ad82eee6
6 changed files with 305 additions and 396 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

180
gtsam/linear/GaussianBayesNetUnordered.cpp
View File

@ -15,105 +15,48 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNetUnordered.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraphUnordered.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/base/timing.h>
#include <gtsam/inference/BayesNet-inl.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp>
#include <stdarg.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) #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) #define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL)
namespace gtsam { namespace gtsam {
// Explicitly instantiate so we don't have to include everywhere
template class BayesNet<GaussianConditional>;
/* ************************************************************************* */ /* ************************************************************************* */
GaussianBayesNet scalarGaussian(Index key, double mu, double sigma) { bool GaussianBayesNetUnordered::equals(const This& bn, double tol = 1e-9) const
GaussianBayesNet bn; {
GaussianConditional::shared_ptr return Base::equals(bn, tol);
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();
}
return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
}
/* ************************************************************************* */
VectorValues optimize(const GaussianBayesNet& bn) {
VectorValues x = *allocateVectorValues(bn);
optimizeInPlace(bn, x);
return x;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValuesUnordered GaussianBayesNetUnordered::optimize() const
{
VectorValuesUnordered soln;
// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x) {
/** solve each node in turn in topological sort order (parents first)*/ /** 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 // 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:)) // (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));
} }
return soln;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& input) { VectorValuesUnordered GaussianBayesNetUnordered::backSubstitute(const VectorValuesUnordered& rhs) const
VectorValues output = input; {
BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) { VectorValuesUnordered result;
const Index key = *(cg->beginFrontals()); BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) {
Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents()); result.insert(cg->solveOtherRHS(result, rhs));
xS = input[key] - cg->get_S() * xS;
output[key] = cg->get_R().triangularView<Eigen::Upper>().solve(xS);
} }
return result;
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
cg->scaleFrontalsBySigma(output);
}
return output;
} }
@ -121,48 +64,32 @@ VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& inpu
// gy=inv(L)*gx by solving L*gy=gx. // gy=inv(L)*gx by solving L*gy=gx.
// gy=inv(R'*inv(Sigma))*gx // gy=inv(R'*inv(Sigma))*gx
// gz'*R'=gx', gy = gz.*sigmas // gz'*R'=gx', gy = gz.*sigmas
VectorValues backSubstituteTranspose(const GaussianBayesNet& bn, VectorValuesUnordered GaussianBayesNetUnordered::backSubstituteTranspose(const VectorValuesUnordered& gx) const
const VectorValues& gx) { {
// Initialize gy from gx // Initialize gy from gx
// TODO: used to insert zeros if gx did not have an entry for a variable in bn // 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 // we loop from first-eliminated to last-eliminated
// i^th part of L*gy=gx is done block-column by block-column of L // 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); cg->solveTransposeInPlace(gy);
// Scale gy
BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn)
cg->scaleFrontalsBySigma(gy);
return gy; return gy;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues optimizeGradientSearch(const GaussianBayesNet& Rd) { VectorValuesUnordered GaussianBayesNetUnordered::optimizeGradientSearch() const
gttic(Allocate_VectorValues); {
VectorValues grad = *allocateVectorValues(Rd);
gttoc(Allocate_VectorValues);
optimizeGradientSearchInPlace(Rd, grad);
return grad;
}
/* ************************************************************************* */
void optimizeGradientSearchInPlace(const GaussianBayesNet& Rd, 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(Rd, grad); VectorValuesUnordered grad = gradientAtZero();
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(Rd); 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);
@ -174,59 +101,14 @@ void optimizeGradientSearchInPlace(const GaussianBayesNet& Rd, VectorValues& gra
// Compute steepest descent point // Compute steepest descent point
scal(step, grad); scal(step, grad);
gttoc(Compute_point); gttoc(Compute_point);
return grad;
} }
/* ************************************************************************* */ /* ************************************************************************* */
pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) { pair<Matrix,Vector> GaussianBayesNetUnordered::matrix() const
{
// add the dimensions of all variables to get matrix dimension return GaussianFactorGraphUnordered(*this).jacobian();
// 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);
}
}
// 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];
// 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);
} }
/* ************************************************************************* */ /* ************************************************************************* */

131
gtsam/linear/GaussianBayesNetUnordered.h
View File

@ -20,54 +20,50 @@
#pragma once #pragma once
#include <gtsam/linear/GaussianConditionalUnordered.h>
#include <gtsam/inference/BayesNetUnordered.h>
#include <gtsam/global_includes.h> #include <gtsam/global_includes.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/inference/BayesNet.h>
namespace gtsam { namespace gtsam {
/** A Bayes net made from linear-Gaussian densities */ /** A Bayes net made from linear-Gaussian densities */
typedef BayesNet<GaussianConditional> GaussianBayesNet; class GTSAM_EXPORT GaussianBayesNetUnordered: public BayesNetUnordered<GaussianConditionalUnordered> {
/** Create a scalar Gaussian */ public:
GTSAM_EXPORT GaussianBayesNet scalarGaussian(Index key, double mu=0.0, double sigma=1.0);
/** Create a simple Gaussian on a single multivariate variable */ typedef BayesNetUnordered<GaussianConditionalUnordered> Base;
GTSAM_EXPORT GaussianBayesNet simpleGaussian(Index key, const Vector& mu, double sigma=1.0); typedef GaussianBayesNetUnordered This;
typedef GaussianConditionalUnordered ConditionalType;
typedef boost::shared_ptr<This> shared_ptr;
/** /// @name Standard Constructors
* 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);
/** /** Construct empty factor graph */
* Add a conditional node with two parents GaussianBayesNetUnordered() {}
* |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 from iterator over conditionals */
* Allocate a VectorValues for the variables in a BayesNet template<typename ITERATOR>
*/ GaussianBayesNetUnordered(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {}
GTSAM_EXPORT boost::shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn);
/// @}
/// @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 * Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by
* back-substitution. * back-substitution.
*/ */
GTSAM_EXPORT VectorValues optimize(const GaussianBayesNet& bn); VectorValuesUnordered optimize() const;
/**
* 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);
/** /**
* Optimize along the gradient direction, with a closed-form computation to * 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 * @param bn The GaussianBayesNet on which to perform this computation
* @return The resulting \f$ \delta x \f$ as described above * @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);
/** ///@name Linear Algebra
* 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);
/** /**
* Return (dense) upper-triangular matrix representation * Return (dense) upper-triangular matrix representation
*/ */
GTSAM_EXPORT std::pair<Matrix, Vector> matrix(const GaussianBayesNet&); std::pair<Matrix, Vector> matrix() 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
*/
GTSAM_EXPORT double determinant(const GaussianBayesNet& bayesNet);
/** /**
* Compute the gradient of the energy function, * Compute the gradient of the energy function,
@ -145,7 +114,7 @@ namespace gtsam {
* @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
*/ */
GTSAM_EXPORT VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0); VectorValuesUnordered gradient(const VectorValuesUnordered& x0) const;
/** /**
* Compute the gradient of the energy function, * 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 * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
* @return The gradient as a VectorValues * @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 } /// namespace gtsam

3
gtsam/linear/GaussianBayesTreeUnordered.cpp
View File

@ -72,8 +72,7 @@ namespace gtsam {
{ {
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)
VectorValuesUnordered grad; VectorValuesUnordered grad = gradientAtZero();
bayesTree.gradientAtZeroInPlace(grad);
double gradientSqNorm = grad.dot(grad); double gradientSqNorm = grad.dot(grad);
gttoc(Compute_Gradient); gttoc(Compute_Gradient);

82
gtsam/linear/GaussianConditionalUnordered.cpp
View File

@ -104,47 +104,83 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianConditionalUnordered::solveInPlace(VectorValuesUnordered& x) const { VectorValuesUnordered GaussianConditionalUnordered::solve(const VectorValuesUnordered& x) const
static const bool debug = false; {
if(debug) this->print("Solving conditional in place"); // Solve matrix
Vector xS = internal::extractVectorValuesSlices(x, this->beginParents(), this->endParents()); Vector xS = x.vector(vector<Key>(beginParents(), endParents()));
xS = this->getb() - this->get_S() * xS; xS = getb() - get_S() * xS;
Vector soln = this->get_R().triangularView<Eigen::Upper>().solve(xS); Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);
// Check for indeterminant solution // Check for indeterminant solution
if(soln.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite<double>), boost::lambda::_1)).any()) 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) { // Insert solution into a VectorValues
gtsam::print(Matrix(this->get_R()), "Calling backSubstituteUpper on "); VectorValuesUnordered result;
gtsam::print(soln, "full back-substitution solution: "); 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 { VectorValuesUnordered GaussianConditionalUnordered::solveOtherRHS(
Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); 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())); frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R()));
// Check for indeterminant solution // Check for indeterminant solution
if(frontalVec.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite<double>), boost::lambda::_1)).any()) if(frontalVec.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite<double>), boost::lambda::_1)).any())
throw IndeterminantLinearSystemException(this->keys().front()); throw IndeterminantLinearSystemException(this->keys().front());
GaussianConditionalUnordered::const_iterator it; for (const_iterator it = beginParents(); it!= endParents(); it++)
for (it = beginParents(); it!= endParents(); it++) { gtsam::transposeMultiplyAdd(-1.0, Matrix(getA(it)), frontalVec, gy[*it]);
const Key i = *it;
gtsam::transposeMultiplyAdd(-1.0, getA(it), frontalVec, gy[i]); // 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 { void GaussianConditionalUnordered::scaleFrontalsBySigma(VectorValuesUnordered& gy) const
Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); {
if(model_) DenseIndex vectorPosition = 0;
frontalVec.array() *= model_->sigmas().array(); for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); gy[*frontal].array() *= model_->sigmas().segment(vectorPosition, getDim(frontal)).array();
vectorPosition += getDim(frontal);
}
} }
} }

14
gtsam/linear/GaussianConditionalUnordered.h
View File

@ -117,17 +117,17 @@ namespace gtsam {
* variables of this conditional, this solve function computes * variables of this conditional, this solve function computes
* \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution. * \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 * @param parents VectorValues containing solved parents \f$ x_s \f$.
* solution \f$ x_f \f$ will be written.
*/ */
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 transpose backsubstition in place on values */
* Performs backsubstition in place on values
*/
void solveTransposeInPlace(VectorValuesUnordered& gy) const; 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; void scaleFrontalsBySigma(VectorValuesUnordered& gy) const;
private: private:

57
gtsam/linear/GaussianFactorGraphUnordered.h
View File

@ -139,13 +139,8 @@ namespace gtsam {
return exp(-0.5 * error(c)); return exp(-0.5 * error(c));
} }
/** ///@name Linear Algebra
* 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);
/** /**
* Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix, * 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; //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: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
@ -273,30 +292,6 @@ namespace gtsam {
/** x += alpha*A'*e */ /** x += alpha*A'*e */
GTSAM_EXPORT void transposeMultiplyAdd(const GaussianFactorGraphUnordered& fg, double alpha, const Errors& e, VectorValuesUnordered& x); 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 */ /* matrix-vector operations */
GTSAM_EXPORT void residual(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r); GTSAM_EXPORT void residual(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r);
GTSAM_EXPORT void multiply(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r); GTSAM_EXPORT void multiply(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r);