diff --git a/gtsam/linear/GaussianBayesNetUnordered.cpp b/gtsam/linear/GaussianBayesNetUnordered.cpp index 8e1520235..5974a968a 100644 --- a/gtsam/linear/GaussianBayesNetUnordered.cpp +++ b/gtsam/linear/GaussianBayesNetUnordered.cpp @@ -15,241 +15,123 @@ * @author Frank Dellaert */ -#include -#include -#include -#include +#include +#include +#include #include -#include -#include - -#include 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; - -/* ************************************************************************* */ -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 allocateVectorValues(const GaussianBayesNet& bn) { - vector dimensions(bn.size()); - Index var = 0; - BOOST_FOREACH(const boost::shared_ptr conditional, bn) { - dimensions[var++] = conditional->dim(); - } - return boost::shared_ptr(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) { - /** solve each node in turn in topological sort order (parents first)*/ - BOOST_REVERSE_FOREACH(const boost::shared_ptr cg, bn) { - // 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); - } -} - -/* ************************************************************************* */ -VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& input) { - VectorValues output = input; - BOOST_REVERSE_FOREACH(const boost::shared_ptr 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().solve(xS); + /* ************************************************************************* */ + bool GaussianBayesNetUnordered::equals(const This& bn, double tol = 1e-9) const + { + return Base::equals(bn, tol); } - BOOST_FOREACH(const boost::shared_ptr cg, bn) { - cg->scaleFrontalsBySigma(output); - } - - 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) { - - // Initialize gy from gx - // TODO: used to insert zeros if gx did not have an entry for a variable in bn - VectorValues 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 cg, bn) - 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) { - gttic(Compute_Gradient); - // Compute gradient (call gradientAtZero function, which is defined for various linear systems) - gradientAtZero(Rd, grad); - double gradientSqNorm = grad.dot(grad); - gttoc(Compute_Gradient); - - gttic(Compute_Rg); - // Compute R * g - FactorGraph Rd_jfg(Rd); - 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_point); - // Compute steepest descent point - scal(step, grad); - gttoc(Compute_point); -} - -/* ************************************************************************* */ -pair 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 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); + /* ************************************************************************* */ + 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 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:)) + soln.insert(cg->solve(soln)); } + return soln; } - // 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 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;iget_R(); - for (size_t i=0;ibeginParents(); - 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 cg, bayesNet){ - logDet += cg->get_R().diagonal().unaryExpr(ptr_fun(log)).sum(); + /* ************************************************************************* */ + 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 exp(logDet); -} -/* ************************************************************************* */ -VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0) { - return gradient(GaussianFactorGraph(bayesNet), x0); -} + /* ************************************************************************* */ + // 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 + VectorValuesUnordered gy = gx; -/* ************************************************************************* */ -void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g) { - gradientAtZero(GaussianFactorGraph(bayesNet), g); -} + // 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 sharedConditional& cg, *this) + cg->solveTransposeInPlace(gy); -/* ************************************************************************* */ + return gy; + } + + /* ************************************************************************* */ + VectorValuesUnordered GaussianBayesNetUnordered::optimizeGradientSearch() const + { + gttic(Compute_Gradient); + // Compute gradient (call gradientAtZero function, which is defined for various linear systems) + VectorValuesUnordered grad = gradientAtZero(); + double gradientSqNorm = grad.dot(grad); + gttoc(Compute_Gradient); + + gttic(Compute_Rg); + // Compute R * g + Errors Rg = GaussianFactorGraphUnordered(*this) * 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_point); + // Compute steepest descent point + scal(step, grad); + gttoc(Compute_point); + + return grad; + } + + /* ************************************************************************* */ + pair GaussianBayesNetUnordered::matrix() const + { + return GaussianFactorGraphUnordered(*this).jacobian(); + } + + /* ************************************************************************* */ + double determinant(const GaussianBayesNet& bayesNet) { + double logDet = 0.0; + + BOOST_FOREACH(boost::shared_ptr cg, bayesNet){ + logDet += cg->get_R().diagonal().unaryExpr(ptr_fun(log)).sum(); + } + + return exp(logDet); + } + + /* ************************************************************************* */ + VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0) { + return gradient(GaussianFactorGraph(bayesNet), x0); + } + + /* ************************************************************************* */ + void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g) { + gradientAtZero(GaussianFactorGraph(bayesNet), g); + } + + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/GaussianBayesNetUnordered.h b/gtsam/linear/GaussianBayesNetUnordered.h index 3052e3926..2e1b1ebbf 100644 --- a/gtsam/linear/GaussianBayesNetUnordered.h +++ b/gtsam/linear/GaussianBayesNetUnordered.h @@ -20,142 +20,139 @@ #pragma once +#include +#include #include -#include -#include namespace gtsam { /** A Bayes net made from linear-Gaussian densities */ - typedef BayesNet GaussianBayesNet; + class GTSAM_EXPORT GaussianBayesNetUnordered: public BayesNetUnordered { - /** 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 Base; + typedef GaussianBayesNetUnordered This; + typedef GaussianConditionalUnordered ConditionalType; + typedef boost::shared_ptr 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 allocateVectorValues(const GaussianBayesNet& bn); + /** Construct from iterator over conditionals */ + template + GaussianBayesNetUnordered(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {} - /** - * 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); + /// @name Testable + /// @{ - /** - * 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] - * - * @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); + /** Check equality */ + bool equals(const This& bn, double tol = 1e-9) 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); + /// @name Standard Interface + /// @{ - /** - * 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); + /** + * Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by + * back-substitution. + */ + VectorValuesUnordered optimize() const; - /** - * Return (dense) upper-triangular matrix representation - */ - GTSAM_EXPORT std::pair matrix(const GaussianBayesNet&); + /** + * 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] + * + * @param bn The GaussianBayesNet on which to perform this computation + * @return The resulting \f$ \delta x \f$ as described above + */ + VectorValuesUnordered optimizeGradientSearch() 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, - * \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 bayesNet The Gaussian Bayes net $(R,d)$ - * @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); + ///@name Linear Algebra + ///@{ + + /** + * Return (dense) upper-triangular matrix representation + */ + std::pair matrix() 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 bayesNet The Gaussian Bayes net $(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 GaussianBayesNet& bayesNet, VectorValues& g); + /** + * 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 bayesNet The Gaussian Bayes net $(R,d)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues + */ + VectorValuesUnordered gradient(const VectorValuesUnordered& x0) 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 bayesNet The Gaussian Bayes net $(R,d)$ + * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues + * @return The gradient as a VectorValues + */ + 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 diff --git a/gtsam/linear/GaussianBayesTreeUnordered.cpp b/gtsam/linear/GaussianBayesTreeUnordered.cpp index e761a14ca..bee502d32 100644 --- a/gtsam/linear/GaussianBayesTreeUnordered.cpp +++ b/gtsam/linear/GaussianBayesTreeUnordered.cpp @@ -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); diff --git a/gtsam/linear/GaussianConditionalUnordered.cpp b/gtsam/linear/GaussianConditionalUnordered.cpp index d89a8d9da..e7cef825e 100644 --- a/gtsam/linear/GaussianConditionalUnordered.cpp +++ b/gtsam/linear/GaussianConditionalUnordered.cpp @@ -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().solve(xS); + VectorValuesUnordered GaussianConditionalUnordered::solve(const VectorValuesUnordered& x) const + { + // Solve matrix + Vector xS = x.vector(vector(beginParents(), endParents())); + xS = getb() - get_S() * xS; + Vector soln = get_R().triangularView().solve(xS); // Check for indeterminant solution if(soln.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite), 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(beginParents(), endParents())); + const Vector rhsR = rhs.vector(vector(beginFrontals(), endFrontals())); + xS = rhsR - get_S() * xS; + Vector soln = get_R().triangularView().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(beginFrontals(), endFrontals())); + frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); // Check for indeterminant solution if(frontalVec.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite), 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); + } } } diff --git a/gtsam/linear/GaussianConditionalUnordered.h b/gtsam/linear/GaussianConditionalUnordered.h index eda9a4c9f..cebbcd52d 100644 --- a/gtsam/linear/GaussianConditionalUnordered.h +++ b/gtsam/linear/GaussianConditionalUnordered.h @@ -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: diff --git a/gtsam/linear/GaussianFactorGraphUnordered.h b/gtsam/linear/GaussianFactorGraphUnordered.h index 245fafad4..60aa25112 100644 --- a/gtsam/linear/GaussianFactorGraphUnordered.h +++ b/gtsam/linear/GaussianFactorGraphUnordered.h @@ -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 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);