From 33d55449de22ab7ff50af9258345ffd184a0d9c5 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 27 Jun 2013 23:03:42 +0000 Subject: [PATCH] Starting work on linear unordered classes --- gtsam/base/VerticalBlockMatrix.cpp | 34 + gtsam/base/VerticalBlockMatrix.h | 223 +++++++ gtsam/linear/GaussianBayesNetUnordered.cpp | 255 ++++++++ gtsam/linear/GaussianBayesNetUnordered.h | 161 +++++ gtsam/linear/GaussianBayesTreeUnordered-inl.h | 59 ++ gtsam/linear/GaussianBayesTreeUnordered.cpp | 96 +++ gtsam/linear/GaussianBayesTreeUnordered.h | 114 ++++ .../linear/GaussianConditionalUnordered-inl.h | 86 +++ gtsam/linear/GaussianConditionalUnordered.cpp | 150 +++++ gtsam/linear/GaussianConditionalUnordered.h | 146 +++++ gtsam/linear/GaussianFactorGraphUnordered.cpp | 591 ++++++++++++++++++ gtsam/linear/GaussianFactorGraphUnordered.h | 384 ++++++++++++ gtsam/linear/GaussianFactorUnordered.h | 98 +++ gtsam/linear/HessianFactorUnordered.cpp | 560 +++++++++++++++++ gtsam/linear/HessianFactorUnordered.h | 377 +++++++++++ gtsam/linear/JacobianFactorUnordered-inl.h | 76 +++ gtsam/linear/JacobianFactorUnordered.cpp | 411 ++++++++++++ gtsam/linear/JacobianFactorUnordered.h | 301 +++++++++ gtsam/linear/VectorValuesUnordered.cpp | 236 +++++++ gtsam/linear/VectorValuesUnordered.h | 455 ++++++++++++++ gtsam/linear/linearExceptions.cpp | 46 ++ 21 files changed, 4859 insertions(+) create mode 100644 gtsam/base/VerticalBlockMatrix.cpp create mode 100644 gtsam/base/VerticalBlockMatrix.h create mode 100644 gtsam/linear/GaussianBayesNetUnordered.cpp create mode 100644 gtsam/linear/GaussianBayesNetUnordered.h create mode 100644 gtsam/linear/GaussianBayesTreeUnordered-inl.h create mode 100644 gtsam/linear/GaussianBayesTreeUnordered.cpp create mode 100644 gtsam/linear/GaussianBayesTreeUnordered.h create mode 100644 gtsam/linear/GaussianConditionalUnordered-inl.h create mode 100644 gtsam/linear/GaussianConditionalUnordered.cpp create mode 100644 gtsam/linear/GaussianConditionalUnordered.h create mode 100644 gtsam/linear/GaussianFactorGraphUnordered.cpp create mode 100644 gtsam/linear/GaussianFactorGraphUnordered.h create mode 100644 gtsam/linear/GaussianFactorUnordered.h create mode 100644 gtsam/linear/HessianFactorUnordered.cpp create mode 100644 gtsam/linear/HessianFactorUnordered.h create mode 100644 gtsam/linear/JacobianFactorUnordered-inl.h create mode 100644 gtsam/linear/JacobianFactorUnordered.cpp create mode 100644 gtsam/linear/JacobianFactorUnordered.h create mode 100644 gtsam/linear/VectorValuesUnordered.cpp create mode 100644 gtsam/linear/VectorValuesUnordered.h create mode 100644 gtsam/linear/linearExceptions.cpp diff --git a/gtsam/base/VerticalBlockMatrix.cpp b/gtsam/base/VerticalBlockMatrix.cpp new file mode 100644 index 000000000..4f10ac038 --- /dev/null +++ b/gtsam/base/VerticalBlockMatrix.cpp @@ -0,0 +1,34 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file VerticalBlockMatrix.cpp + * @brief A matrix with column blocks of pre-defined sizes. Used in JacobianFactor and + * GaussianConditional. + * @author Richard Roberts + * @date Sep 18, 2010 */ + +#include + +namespace gtsam { + + /* ************************************************************************* */ + VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& rhs) { + VerticalBlockMatrix result; + result.variableColOffsets_.resize(rhs.nBlocks() + 1); + for(Index i = 0; i < rhs.nBlocks(); ++i) + result.variableColOffsets_[i] = rhs.variableColOffsets_[i + rhs.blockStart_]; + result.matrix_.resize(rhs.rows(), result.variableColOffsets_.back()); + result.rowEnd_ = rhs.rows(); + result.assertInvariants(); + } + +} diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h new file mode 100644 index 000000000..c38199cb2 --- /dev/null +++ b/gtsam/base/VerticalBlockMatrix.h @@ -0,0 +1,223 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file VerticalBlockMatrix.h + * @brief A matrix with column blocks of pre-defined sizes. Used in JacobianFactor and + * GaussianConditional. + * @author Richard Roberts + * @date Sep 18, 2010 */ +#pragma once + +#include + +namespace gtsam { + + // Forward declarations + //class S + + /** + * This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks. + * It also provides for accessing individual columns from those blocks. When constructed or + * resized, the caller must provide the dimensions of the blocks. + * + * This class also has three parameters that can be changed after construction that change the + * apparent view of the matrix without any reallocation or data copying. firstBlock() determines + * the block that has index 0 for all operations (except for re-setting firstBlock()). rowStart() + * determines the apparent first row of the matrix for all operations (except for setting rowStart() + * and rowEnd()). rowEnd() determines the apparent exclusive (one-past-the-last) last row for all + * operations. To include all rows, rowEnd() should be set to the number of rows in the matrix + * (i.e. one after the last true row index). + * + * @addtogroup base */ + class VerticalBlockMatrix { + public: + typedef VerticalBlockMatrix This; + typedef Matrix::Index Index; + typedef Eigen::Block Block; + typedef Eigen::Block constBlock; + + // columns of blocks + typedef Eigen::Block::ColXpr Column; + typedef Eigen::Block::ConstColXpr constColumn; + + protected: + Matrix matrix_; ///< The full matrix + std::vector variableColOffsets_; ///< the starting columns of each block (0-based) + + Index rowStart_; ///< Changes apparent matrix view, see main class comment. + Index rowEnd_; ///< Changes apparent matrix view, see main class comment. + Index blockStart_; ///< Changes apparent matrix view, see main class comment. + + public: + + /** Construct an empty VerticalBlockMatrix */ + VerticalBlockMatrix() : + rowStart_(0), rowEnd_(0), blockStart_(0) + { + variableColOffsets_.push_back(0); + } + + /** + * Construct from a container of the sizes of each vertical block, resize the matrix so that its + * height is matrixNewHeight and its width fits the given block dimensions. */ + template + VerticalBlockMatrix(const CONTAINER dimensions, int height) : + matrix_(matrixNewHeight, variableColOffsets_.back()), + rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0) + { + fillOffsets(firstBlockDim, lastBlockDim); + assertInvariants(); + } + + /** + * Construct from iterator over the sizes of each vertical block, resize the matrix so that its + * height is matrixNewHeight and its width fits the given block dimensions. */ + template + VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, int height) : + matrix_(matrixNewHeight, variableColOffsets_.back()), + rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0) + { + fillOffsets(firstBlockDim, lastBlockDim); + assertInvariants(); + } + + /// Row size + Index rows() const { assertInvariants(); return rowEnd_ - rowStart_; } + + /// Column size + Index cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } + + /// Block count + Index nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } + + /** Access a single block in the underlying matrix with read/write access */ + Block operator()(Index block) { return range(block, block+1); } + + /** Access a const block view */ + const constBlock operator()(Index block) const { return range(block, block+1); } + + /** access ranges of blocks at a time */ + Block range(Index startBlock, Index endBlock) { + assertInvariants(); + Index actualStartBlock = startBlock + blockStart_; + Index actualEndBlock = endBlock + blockStart_; + if(startBlock != 0 || endBlock != 0) { + checkBlock(actualStartBlock); + checkBlock(actualEndBlock); + } + const Index startCol = variableColOffsets_[actualStartBlock]; + const Index rangeCols = variableColOffsets_[actualEndBlock] - startCol; + return matrix_.block(rowStart_, startCol, this->rows(), rangeCols); + } + + const constBlock range(Index startBlock, Index endBlock) const { + assertInvariants(); + Index actualStartBlock = startBlock + blockStart_; + Index actualEndBlock = endBlock + blockStart_; + if(startBlock != 0 || endBlock != 0) { + checkBlock(actualStartBlock); + checkBlock(actualEndBlock); + } + const Index startCol = variableColOffsets_[actualStartBlock]; + const Index rangeCols = variableColOffsets_[actualEndBlock] - startCol; + return ((const Matrix&)matrix_).block(rowStart_, startCol, this->rows(), rangeCols); + } + + /** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */ + Block full() { return range(0, nBlocks()); } + + /** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */ + const constBlock full() const { return range(0, nBlocks()); } + + Index offset(Index block) const { + assertInvariants(); + Index actualBlock = block + blockStart_; + checkBlock(actualBlock); + return variableColOffsets_[actualBlock]; + } + + /** Get or set the apparent first row of the underlying matrix for all operations */ + Index& rowStart() { return rowStart_; } + + /** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ + Index& rowEnd() { return rowEnd_; } + + /** Get or set the apparent first block for all operations */ + Index& firstBlock() { return blockStart_; } + + /** Get the apparent first row of the underlying matrix for all operations */ + Index rowStart() const { return rowStart_; } + + /** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ + Index rowEnd() const { return rowEnd_; } + + /** Get the apparent first block for all operations */ + Index firstBlock() const { return blockStart_; } + + /** Access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */ + const Matrix& matrix() const { return matrix_; } + + /** Non-const access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */ + Matrix& matrix() { return matrix_; } + + /** + * Copy the block structure and resize the underlying matrix, but do not + * copy the matrix data. If blockStart(), rowStart(), and/or rowEnd() have + * been modified, this copies the structure of the corresponding matrix view. + * In the destination VerticalBlockView, blockStart() and rowStart() will + * thus be 0, rowEnd() will be cols() of the source VerticalBlockView, and + * the underlying matrix will be the size of the view of the source matrix. + */ + static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs); + + protected: + void assertInvariants() const { + assert(matrix_.cols() == variableColOffsets_.back()); + assert(blockStart_ < variableColOffsets_.size()); + assert(rowStart_ <= matrix_.rows()); + assert(rowEnd_ <= matrix_.rows()); + assert(rowStart_ <= rowEnd_); + } + + void checkBlock(Index block) const { + assert(matrix_.cols() == variableColOffsets_.back()); + assert(block < variableColOffsets_.size()-1); + assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols()); + } + + template + void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) { + variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1); + variableColOffsets_[0] = 0; + Index j=0; + for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { + variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; + ++ j; + } + } + + //friend class SymmetricBlockView; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); + ar & BOOST_SERIALIZATION_NVP(rowStart_); + ar & BOOST_SERIALIZATION_NVP(rowEnd_); + ar & BOOST_SERIALIZATION_NVP(blockStart_); + } + }; + +} diff --git a/gtsam/linear/GaussianBayesNetUnordered.cpp b/gtsam/linear/GaussianBayesNetUnordered.cpp new file mode 100644 index 000000000..8e1520235 --- /dev/null +++ b/gtsam/linear/GaussianBayesNetUnordered.cpp @@ -0,0 +1,255 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GaussianBayesNet.cpp + * @brief Chordal Bayes Net, the result of eliminating a factor graph + * @author Frank Dellaert + */ + +#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); + } + + 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); + } + } + + // 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(); + } + + 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 new file mode 100644 index 000000000..3052e3926 --- /dev/null +++ b/gtsam/linear/GaussianBayesNetUnordered.h @@ -0,0 +1,161 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GaussianBayesNet.h + * @brief Chordal Bayes Net, the result of eliminating a factor graph + * @brief GaussianBayesNet + * @author Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** A Bayes net made from linear-Gaussian densities */ + typedef BayesNet GaussianBayesNet; + + /** Create a scalar Gaussian */ + GTSAM_EXPORT GaussianBayesNet scalarGaussian(Index key, double mu=0.0, double sigma=1.0); + + /** Create a simple Gaussian on a single multivariate variable */ + GTSAM_EXPORT GaussianBayesNet simpleGaussian(Index key, const Vector& mu, double sigma=1.0); + + /** + * 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); + + /** + * 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); + + /** + * Allocate a VectorValues for the variables in a BayesNet + */ + GTSAM_EXPORT boost::shared_ptr allocateVectorValues(const GaussianBayesNet& bn); + + /** + * 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); + + /** + * 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); + + /** 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); + + /** + * Return (dense) upper-triangular matrix representation + */ + GTSAM_EXPORT std::pair 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); + + /** + * 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); + + /** + * 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); + +} /// namespace gtsam diff --git a/gtsam/linear/GaussianBayesTreeUnordered-inl.h b/gtsam/linear/GaussianBayesTreeUnordered-inl.h new file mode 100644 index 000000000..1f6c8e9f5 --- /dev/null +++ b/gtsam/linear/GaussianBayesTreeUnordered-inl.h @@ -0,0 +1,59 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GaussianBayesTree-inl.h + * @brief Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree + * @brief GaussianBayesTree + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include + +#include // Only to help Eclipse + +#include + +namespace gtsam { + +/* ************************************************************************* */ +namespace internal { +template +void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result) { + // parents are assumed to already be solved and available in result + clique->conditional()->solveInPlace(result); + + // starting from the root, call optimize on each conditional + BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_) + optimizeInPlace(child, result); +} + +/* ************************************************************************* */ +template +double logDeterminant(const typename BAYESTREE::sharedClique& clique) { + double result = 0.0; + + // this clique + result += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun(log)).sum(); + + // sum of children + BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_) + result += logDeterminant(child); + + return result; +} + +/* ************************************************************************* */ +} // \namespace internal +} // \namespace gtsam diff --git a/gtsam/linear/GaussianBayesTreeUnordered.cpp b/gtsam/linear/GaussianBayesTreeUnordered.cpp new file mode 100644 index 000000000..7011524e4 --- /dev/null +++ b/gtsam/linear/GaussianBayesTreeUnordered.cpp @@ -0,0 +1,96 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GaussianBayesTree.cpp + * @brief Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree + * @brief GaussianBayesTree + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +VectorValues optimize(const GaussianBayesTree& bayesTree) { + VectorValues result = *allocateVectorValues(bayesTree); + optimizeInPlace(bayesTree, result); + return result; +} + +/* ************************************************************************* */ +void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) { + internal::optimizeInPlace(bayesTree.root(), result); +} + +/* ************************************************************************* */ +VectorValues optimizeGradientSearch(const GaussianBayesTree& bayesTree) { + 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); + // Compute gradient (call gradientAtZero function, which is defined for various linear systems) + gradientAtZero(bayesTree, grad); + double gradientSqNorm = grad.dot(grad); + gttoc(Compute_Gradient); + + gttic(Compute_Rg); + // Compute R * g + FactorGraph Rd_jfg(bayesTree); + 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); +} + +/* ************************************************************************* */ +VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0) { + return gradient(FactorGraph(bayesTree), x0); +} + +/* ************************************************************************* */ +void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g) { + gradientAtZero(FactorGraph(bayesTree), g); +} + +/* ************************************************************************* */ +double determinant(const GaussianBayesTree& bayesTree) { + if (!bayesTree.root()) + return 0.0; + + return exp(internal::logDeterminant(bayesTree.root())); +} +/* ************************************************************************* */ + +} // \namespace gtsam + + + + diff --git a/gtsam/linear/GaussianBayesTreeUnordered.h b/gtsam/linear/GaussianBayesTreeUnordered.h new file mode 100644 index 000000000..9df3d67b4 --- /dev/null +++ b/gtsam/linear/GaussianBayesTreeUnordered.h @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GaussianBayesTree.h + * @brief Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree + * @brief GaussianBayesTree + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/// A Bayes Tree representing a Gaussian density +GTSAM_EXPORT typedef BayesTree GaussianBayesTree; + +/// optimize the BayesTree, starting from the root +GTSAM_EXPORT VectorValues optimize(const GaussianBayesTree& bayesTree); + +/// recursively optimize this conditional and all subtrees +GTSAM_EXPORT void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result); + +namespace internal { +template +void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result); +} + +/** + * 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] + */ +GTSAM_EXPORT VectorValues optimizeGradientSearch(const GaussianBayesTree& bayesTree); + +/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ +GTSAM_EXPORT void optimizeGradientSearchInPlace(const GaussianBayesTree& bayesTree, VectorValues& grad); + +/** + * 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 bayesTree The Gaussian Bayes Tree $(R,d)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues + */ +GTSAM_EXPORT VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0); + +/** + * Compute the gradient of the energy function, + * \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 bayesTree The Gaussian Bayes Tree $(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 GaussianBayesTree& bayesTree, VectorValues& g); + +/** + * Computes the determinant of a GassianBayesTree + * A GassianBayesTree is an upper triangular matrix and for an upper triangular matrix + * determinant is the product of the diagonal elements. Instead of actually multiplying + * we add the logarithms of the diagonal elements and take the exponent at the end + * because this is more numerically stable. + * @param bayesTree The input GassianBayesTree + * @return The determinant + */ +GTSAM_EXPORT double determinant(const GaussianBayesTree& bayesTree); + + +namespace internal { +template +double logDeterminant(const typename BAYESTREE::sharedClique& clique); +} + +} + +#include + diff --git a/gtsam/linear/GaussianConditionalUnordered-inl.h b/gtsam/linear/GaussianConditionalUnordered-inl.h new file mode 100644 index 000000000..0f29f5957 --- /dev/null +++ b/gtsam/linear/GaussianConditionalUnordered-inl.h @@ -0,0 +1,86 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GaussianConditional-inl.h + * @brief Conditional Gaussian Base class + * @author Christian Potthast + */ + +// \callgraph + +#pragma once + +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + GaussianConditionalUnordered::GaussianConditionalUnordered(Index key, const Vector& d, + const Matrix& R, const PARENTS& parents, const SharedDiagonal& sigmas) : + BaseFactor(boost::join(boost::assign::cref_list_of<1>(std::make_pair(key, R)), parents), d, sigmas), + BaseConditional(1) {} + + /* ************************************************************************* */ + template + GaussianConditionalUnordered::GaussianConditionalUnordered(const TERMS& terms, + size_t nrFrontals, const Vector& d, const SharedDiagonal& sigmas) : + BaseFactor(terms, d, sigmas), BaseConditional(nrFrontals) {} + + /* ************************************************************************* */ + template + GaussianConditional::shared_ptr GaussianConditional::Combine(ITERATOR firstConditional, ITERATOR lastConditional) { + + // TODO: check for being a clique + + // Get dimensions from first conditional + std::vector dims; dims.reserve((*firstConditional)->size() + 1); + for(const_iterator j = (*firstConditional)->begin(); j != (*firstConditional)->end(); ++j) + dims.push_back((*firstConditional)->dim(j)); + dims.push_back(1); + + // We assume the conditionals form clique, so the first n variables will be + // frontal variables in the new conditional. + size_t nFrontals = 0; + size_t nRows = 0; + for(ITERATOR c = firstConditional; c != lastConditional; ++c) { + nRows += dims[nFrontals]; + ++ nFrontals; + } + + // Allocate combined conditional, has same keys as firstConditional + Matrix tempCombined; + VerticalBlockView tempBlockView(tempCombined, dims.begin(), dims.end(), 0); + GaussianConditional::shared_ptr combinedConditional(new GaussianConditional((*firstConditional)->begin(), (*firstConditional)->end(), nFrontals, tempBlockView, zero(nRows))); + + // Resize to correct number of rows + combinedConditional->matrix_.resize(nRows, combinedConditional->matrix_.cols()); + combinedConditional->rsd_.rowEnd() = combinedConditional->matrix_.rows(); + + // Copy matrix and sigmas + const size_t totalDims = combinedConditional->matrix_.cols(); + size_t currentSlot = 0; + for(ITERATOR c = firstConditional; c != lastConditional; ++c) { + const size_t startRow = combinedConditional->rsd_.offset(currentSlot); // Start row is same as start column + combinedConditional->rsd_.range(0, currentSlot).block(startRow, 0, dims[currentSlot], combinedConditional->rsd_.offset(currentSlot)).operator=( + Matrix::Zero(dims[currentSlot], combinedConditional->rsd_.offset(currentSlot))); + combinedConditional->rsd_.range(currentSlot, dims.size()).block(startRow, 0, dims[currentSlot], totalDims - startRow).operator=( + (*c)->matrix_); + combinedConditional->sigmas_.segment(startRow, dims[currentSlot]) = (*c)->sigmas_; + ++ currentSlot; + } + + return combinedConditional; + } + +} // gtsam diff --git a/gtsam/linear/GaussianConditionalUnordered.cpp b/gtsam/linear/GaussianConditionalUnordered.cpp new file mode 100644 index 000000000..d88629b6d --- /dev/null +++ b/gtsam/linear/GaussianConditionalUnordered.cpp @@ -0,0 +1,150 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GaussianConditional.cpp + * @brief Conditional Gaussian Base class + * @author Christian Potthast + */ + +#include +#include +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#endif +#include +#include +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + +#include +#include +#include + +using namespace std; + +namespace gtsam { + + /* ************************************************************************* */ + GaussianConditionalUnordered::GaussianConditionalUnordered( + Index key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas) : + BaseFactor(key, R, d, sigmas), BaseConditional(1) {} + + /* ************************************************************************* */ + GaussianConditionalUnordered::GaussianConditionalUnordered( + Index key, const Vector& d, const Matrix& R, + Index name1, const Matrix& S, const SharedDiagonal& sigmas) : + BaseFactor(key, R, name1, S, d, sigmas) {} + + /* ************************************************************************* */ + GaussianConditionalUnordered::GaussianConditionalUnordered( + Index key, const Vector& d, const Matrix& R, + Index name1, const Matrix& S, Index name2, const Matrix& T, const SharedDiagonal& sigmas) : + BaseFactor(key, R, name1, S, name2, T, d, sigmas) {} + + /* ************************************************************************* */ + void GaussianConditionalUnordered::print(const string &s, const IndexFormatter& formatter) const + { + cout << s << ": density on "; + for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) { + cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; + } + cout << endl; + gtsam::print(Matrix(get_R()),"R"); + for(const_iterator it = beginParents() ; it != endParents() ; ++it ) { + gtsam::print(Matrix(getA(it)), (boost::format("S[%1%]")%(formatter(*it))).str()); + } + gtsam::print(Vector(getb()),"d"); + model_->print("sigmas"); + } + + /* ************************************************************************* */ + bool GaussianConditionalUnordered::equals(const GaussianConditionalUnordered &c, double tol) const { + // check if the size of the parents_ map is the same + if (parents().size() != c.parents().size()) + return false; + + // check if R_ and d_ are linear independent + for (size_t i=0; i rows1; rows1.push_back(Vector(get_R().row(i))); + list rows2; rows2.push_back(Vector(c.get_R().row(i))); + + // check if the matrices are the same + // iterate over the parents_ map + for (const_iterator it = beginParents(); it != endParents(); ++it) { + const_iterator it2 = c.beginParents() + (it-beginParents()); + if(*it != *(it2)) + return false; + rows1.push_back(row(getA(it), i)); + rows2.push_back(row(c.getA(it2), i)); + } + + Vector row1 = concatVectors(rows1); + Vector row2 = concatVectors(rows2); + if (!linear_dependent(row1, row2, tol)) + return false; + } + + // check if sigmas are equal + if ((model_ && !c.model_) || (!model_ && c.model_) + || !model_->equals(*c.model_, tol)) + return false; + + return true; + } + + /* ************************************************************************* */ + 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); + + // Check for indeterminant solution + if(soln.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite), boost::lambda::_1)).any()) + throw IndeterminantLinearSystemException(this->keys().front()); + + if(debug) { + gtsam::print(Matrix(this->get_R()), "Calling backSubstituteUpper on "); + gtsam::print(soln, "full back-substitution solution: "); + } + internal::writeVectorValuesSlices(soln, x, this->beginFrontals(), this->endFrontals()); + } + + /* ************************************************************************* */ + void GaussianConditionalUnordered::solveTransposeInPlace(VectorValuesUnordered& gy) const { + Vector frontalVec = internal::extractVectorValuesSlices(gy, 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 Index i = *it; + transposeMultiplyAdd(-1.0,getA(it),frontalVec,gy[i]); + } + internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); + } + + /* ************************************************************************* */ + void GaussianConditionalUnordered::scaleFrontalsBySigma(VectorValuesUnordered& gy) const { + Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); + frontalVec = emul(frontalVec, get_sigmas()); + internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); + } + +} + diff --git a/gtsam/linear/GaussianConditionalUnordered.h b/gtsam/linear/GaussianConditionalUnordered.h new file mode 100644 index 000000000..ef1deb1d0 --- /dev/null +++ b/gtsam/linear/GaussianConditionalUnordered.h @@ -0,0 +1,146 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GaussianConditional.h + * @brief Conditional Gaussian Base class + * @author Christian Potthast + */ + +// \callgraph + +#pragma once + +#include + +#include +#include +#include +#include + +namespace gtsam { + + /** + * A conditional Gaussian functions as the node in a Bayes network + * It has a set of parents y,z, etc. and implements a probability density on x. + * The negative log-probability is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$ + */ + class GTSAM_EXPORT GaussianConditionalUnordered : + public JacobianFactorUnordered, + public ConditionalUnordered + { + public: + typedef GaussianConditionalUnordered This; ///< Typedef to this class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef JacobianFactorUnordered BaseFactor; ///< Typedef to our factor base class + typedef ConditionalUnordered BaseConditional; ///< Typedef to our conditional base class + + /** default constructor needed for serialization */ + GaussianConditionalUnordered() {} + + /** constructor with no parents + * |Rx-d| + */ + GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas); + + /** constructor with only one parent + * |Rx+Sy-d| + */ + GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R, + Index name1, const Matrix& S, const SharedDiagonal& sigmas); + + /** constructor with two parents + * |Rx+Sy+Tz-d| + */ + GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R, + Index name1, const Matrix& S, Index name2, const Matrix& T, const SharedDiagonal& sigmas); + + /** + * Constructor with number of arbitrary parents. \f$ |Rx+sum(Ai*xi)-d| \f$ + * @tparam PARENTS A container whose value type is std::pair, specifying the + * collection of parent keys and matrices. */ + template + GaussianConditionalUnordered(Index key, const Vector& d, + const Matrix& R, const PARENTS& parents, const SharedDiagonal& sigmas); + + /** + * Constructor with arbitrary number of frontals and parents. + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the conditional. */ + template + GaussianConditionalUnordered(const TERMS& terms, + size_t nrFrontals, const Vector& d, const SharedDiagonal& sigmas); + + /** Combine several GaussianConditional into a single dense GC. The conditionals enumerated by + * \c first and \c last must be in increasing order, meaning that the parents of any + * conditional may not include a conditional coming before it. + * @param firstConditional Iterator to the first conditional to combine, must dereference to a + * shared_ptr. + * @param lastConditional Iterator to after the last conditional to combine, must dereference + * to a shared_ptr. */ + template + static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional); + + /** print */ + void print(const std::string& = "GaussianConditional", + const IndexFormatter& formatter = DefaultIndexFormatter) const; + + /** equals function */ + bool equals(const GaussianConditionalUnordered&cg, double tol = 1e-9) const; + + /** Return a view of the upper-triangular R block of the conditional */ + constABlock get_R() const { return Ab_.range(0, nrFrontals()); } + + /** Get a view of the parent block corresponding to the variable pointed to by the given key iterator (non-const version) */ + constABlock get_S() const { return Ab_.range(nrFrontals(), size()); } + + public: + /** + * Solves a conditional Gaussian and writes the solution into the entries of + * \c x for each frontal variable of the conditional. The parents are + * assumed to have already been solved in and their values are read from \c x. + * This function works for multiple frontal variables. + * + * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2, + * where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator + * 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. + */ + void solveInPlace(VectorValuesUnordered& x) const; + + // functions for transpose backsubstitution + + /** + * Performs backsubstition in place on values + */ + void solveTransposeInPlace(VectorValuesUnordered& gy) const; + void scaleFrontalsBySigma(VectorValuesUnordered& gy) const; + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(IndexConditional); + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(rsd_); + ar & BOOST_SERIALIZATION_NVP(sigmas_); + } + }; // GaussianConditionalUnordered + +} // gtsam + +#include + diff --git a/gtsam/linear/GaussianFactorGraphUnordered.cpp b/gtsam/linear/GaussianFactorGraphUnordered.cpp new file mode 100644 index 000000000..b04287288 --- /dev/null +++ b/gtsam/linear/GaussianFactorGraphUnordered.cpp @@ -0,0 +1,591 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GaussianFactorGraph.cpp + * @brief Linear Factor Graph where all factors are Gaussians + * @author Kai Ni + * @author Christian Potthast + * @author Richard Roberts + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +namespace gtsam { + + /* ************************************************************************* */ + GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : Base(CBN) {} + + /* ************************************************************************* */ + GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { + FastSet keys; + BOOST_FOREACH(const sharedFactor& factor, *this) + if (factor) keys.insert(factor->begin(), factor->end()); + return keys; + } + + /* ************************************************************************* */ + void GaussianFactorGraph::permuteWithInverse( + const Permutation& inversePermutation) { + BOOST_FOREACH(const sharedFactor& factor, factors_) { + if(factor) + factor->permuteWithInverse(inversePermutation); + } + } + + /* ************************************************************************* */ + void GaussianFactorGraph::reduceWithInverse( + const internal::Reduction& inverseReduction) { + BOOST_FOREACH(const sharedFactor& factor, factors_) { + if(factor) + factor->reduceWithInverse(inverseReduction); + } + } + + /* ************************************************************************* */ + void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg) { + for (const_iterator factor = lfg.factors_.begin(); factor + != lfg.factors_.end(); factor++) { + push_back(*factor); + } + } + + /* ************************************************************************* */ + GaussianFactorGraph GaussianFactorGraph::combine2( + const GaussianFactorGraph& lfg1, const GaussianFactorGraph& lfg2) { + + // create new linear factor graph equal to the first one + GaussianFactorGraph fg = lfg1; + + // add the second factors_ in the graph + for (const_iterator factor = lfg2.factors_.begin(); factor + != lfg2.factors_.end(); factor++) { + fg.push_back(*factor); + } + return fg; + } + + /* ************************************************************************* */ + std::vector > GaussianFactorGraph::sparseJacobian() const { + // First find dimensions of each variable + FastVector dims; + BOOST_FOREACH(const sharedFactor& factor, *this) { + for(GaussianFactor::const_iterator pos = factor->begin(); pos != factor->end(); ++pos) { + if(dims.size() <= *pos) + dims.resize(*pos + 1, 0); + dims[*pos] = factor->getDim(pos); + } + } + + // Compute first scalar column of each variable + vector columnIndices(dims.size()+1, 0); + for(size_t j=1; j triplet; + FastVector entries; + size_t row = 0; + BOOST_FOREACH(const sharedFactor& factor, *this) { + // Convert to JacobianFactor if necessary + JacobianFactor::shared_ptr jacobianFactor( + boost::dynamic_pointer_cast(factor)); + if (!jacobianFactor) { + HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast(factor)); + if (hessian) + jacobianFactor.reset(new JacobianFactor(*hessian)); + else + throw invalid_argument( + "GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor."); + } + + // Whiten the factor and add entries for it + // iterate over all variables in the factor + const JacobianFactor whitened(jacobianFactor->whiten()); + for(JacobianFactor::const_iterator pos=whitened.begin(); pos 1e-12) entries.push_back( + boost::make_tuple(row+i, column_start+j, s)); + } + } + + JacobianFactor::constBVector whitenedb(whitened.getb()); + size_t bcolumn = columnIndices.back(); + for (size_t i = 0; i < (size_t) whitenedb.size(); i++) + entries.push_back(boost::make_tuple(row+i, bcolumn, whitenedb(i))); + + // Increment row index + row += jacobianFactor->rows(); + } + return vector(entries.begin(), entries.end()); + } + + /* ************************************************************************* */ + Matrix GaussianFactorGraph::sparseJacobian_() const { + + // call sparseJacobian + typedef boost::tuple triplet; + std::vector result = sparseJacobian(); + + // translate to base 1 matrix + size_t nzmax = result.size(); + Matrix IJS(3,nzmax); + for (size_t k = 0; k < result.size(); k++) { + const triplet& entry = result[k]; + IJS(0,k) = double(entry.get<0>() + 1); + IJS(1,k) = double(entry.get<1>() + 1); + IJS(2,k) = entry.get<2>(); + } + return IJS; + } + + /* ************************************************************************* */ + Matrix GaussianFactorGraph::augmentedJacobian() const { + // Convert to Jacobians + FactorGraph jfg; + jfg.reserve(this->size()); + BOOST_FOREACH(const sharedFactor& factor, *this) { + if(boost::shared_ptr jf = + boost::dynamic_pointer_cast(factor)) + jfg.push_back(jf); + else + jfg.push_back(boost::make_shared(*factor)); + } + // combine all factors + JacobianFactor combined(*CombineJacobians(jfg, VariableSlots(*this))); + return combined.matrix_augmented(); + } + + /* ************************************************************************* */ + std::pair GaussianFactorGraph::jacobian() const { + Matrix augmented = augmentedJacobian(); + return make_pair( + augmented.leftCols(augmented.cols()-1), + augmented.col(augmented.cols()-1)); + } + + /* ************************************************************************* */ + // Helper functions for Combine + static boost::tuple, size_t, size_t> countDims(const FactorGraph& factors, const VariableSlots& variableSlots) { +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS + vector varDims(variableSlots.size(), numeric_limits::max()); +#else + vector varDims(variableSlots.size()); +#endif + size_t m = 0; + size_t n = 0; + { + Index jointVarpos = 0; + BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) { + + assert(slots.second.size() == factors.size()); + + Index sourceFactorI = 0; + BOOST_FOREACH(const Index sourceVarpos, slots.second) { + if(sourceVarpos < numeric_limits::max()) { + const JacobianFactor& sourceFactor = *factors[sourceFactorI]; + size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos); +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS +if(varDims[jointVarpos] == numeric_limits::max()) { + varDims[jointVarpos] = vardim; + n += vardim; +} else + assert(varDims[jointVarpos] == vardim); +#else + varDims[jointVarpos] = vardim; +n += vardim; +break; +#endif + } + ++ sourceFactorI; + } + ++ jointVarpos; + } + BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) { + m += factor->rows(); + } + } + return boost::make_tuple(varDims, m, n); + } + + /* ************************************************************************* */ + JacobianFactor::shared_ptr CombineJacobians( + const FactorGraph& factors, + const VariableSlots& variableSlots) { + + const bool debug = ISDEBUG("CombineJacobians"); + if (debug) factors.print("Combining factors: "); + if (debug) variableSlots.print(); + + if (debug) cout << "Determine dimensions" << endl; + gttic(countDims); + vector varDims; + size_t m, n; + boost::tie(varDims, m, n) = countDims(factors, variableSlots); + if (debug) { + cout << "Dims: " << m << " x " << n << "\n"; + BOOST_FOREACH(const size_t dim, varDims) cout << dim << " "; + cout << endl; + } + gttoc(countDims); + + if (debug) cout << "Allocate new factor" << endl; + gttic(allocate); + JacobianFactor::shared_ptr combined(new JacobianFactor()); + combined->allocate(variableSlots, varDims, m); + Vector sigmas(m); + gttoc(allocate); + + if (debug) cout << "Copy blocks" << endl; + gttic(copy_blocks); + // Loop over slots in combined factor + Index combinedSlot = 0; + BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) { + JacobianFactor::ABlock destSlot(combined->getA(combined->begin()+combinedSlot)); + // Loop over source factors + size_t nextRow = 0; + for(size_t factorI = 0; factorI < factors.size(); ++factorI) { + // Slot in source factor + const Index sourceSlot = varslot.second[factorI]; + const size_t sourceRows = factors[factorI]->rows(); + JacobianFactor::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows)); + // Copy if exists in source factor, otherwise set zero + if(sourceSlot != numeric_limits::max()) + destBlock = factors[factorI]->getA(factors[factorI]->begin()+sourceSlot); + else + destBlock.setZero(); + nextRow += sourceRows; + } + ++combinedSlot; + } + gttoc(copy_blocks); + + if (debug) cout << "Copy rhs (b) and sigma" << endl; + gttic(copy_vectors); + bool anyConstrained = false; + // Loop over source factors + size_t nextRow = 0; + for(size_t factorI = 0; factorI < factors.size(); ++factorI) { + const size_t sourceRows = factors[factorI]->rows(); + combined->getb().segment(nextRow, sourceRows) = factors[factorI]->getb(); + sigmas.segment(nextRow, sourceRows) = factors[factorI]->get_model()->sigmas(); + if (factors[factorI]->isConstrained()) anyConstrained = true; + nextRow += sourceRows; + } + gttoc(copy_vectors); + + if (debug) cout << "Create noise model from sigmas" << endl; + gttic(noise_model); + combined->setModel(anyConstrained, sigmas); + gttoc(noise_model); + + if (debug) cout << "Assert Invariants" << endl; + combined->assertInvariants(); + + return combined; + } + + /* ************************************************************************* */ + GaussianFactorGraph::EliminationResult EliminateJacobians(const FactorGraph< + JacobianFactor>& factors, size_t nrFrontals) { + gttic(Combine); + JacobianFactor::shared_ptr jointFactor = + CombineJacobians(factors, VariableSlots(factors)); + gttoc(Combine); + gttic(eliminate); + GaussianConditional::shared_ptr gbn = jointFactor->eliminate(nrFrontals); + gttoc(eliminate); + return make_pair(gbn, jointFactor); + } + + /* ************************************************************************* */ + Matrix GaussianFactorGraph::augmentedHessian() const { + // combine all factors and get upper-triangular part of Hessian + HessianFactor combined(*this); + Matrix result = combined.info(); + // Fill in lower-triangular part of Hessian + result.triangularView() = result.transpose(); + return result; + } + + /* ************************************************************************* */ + std::pair GaussianFactorGraph::hessian() const { + Matrix augmented = augmentedHessian(); + return make_pair( + augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1), + augmented.col(augmented.rows()-1).head(augmented.rows()-1)); + } + + /* ************************************************************************* */ + GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< + GaussianFactor>& factors, size_t nrFrontals) { + + const bool debug = ISDEBUG("EliminateCholesky"); + + // Form Ab' * Ab + gttic(combine); + HessianFactor::shared_ptr combinedFactor(new HessianFactor(factors)); + gttoc(combine); + + // Do Cholesky, note that after this, the lower triangle still contains + // some untouched non-zeros that should be zero. We zero them while + // extracting submatrices next. + gttic(partial_Cholesky); + combinedFactor->partialCholesky(nrFrontals); + + gttoc(partial_Cholesky); + + // Extract conditional and fill in details of the remaining factor + gttic(split); + GaussianConditional::shared_ptr conditional = + combinedFactor->splitEliminatedFactor(nrFrontals); + if (debug) { + conditional->print("Extracted conditional: "); + combinedFactor->print("Eliminated factor (L piece): "); + } + gttoc(split); + + combinedFactor->assertInvariants(); + return make_pair(conditional, combinedFactor); + } + + /* ************************************************************************* */ + static FactorGraph convertToJacobians(const FactorGraph< + GaussianFactor>& factors) { + + typedef JacobianFactor J; + typedef HessianFactor H; + + const bool debug = ISDEBUG("convertToJacobians"); + + FactorGraph jacobians; + jacobians.reserve(factors.size()); + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) + if (factor) { + J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); + if (jacobian) { + jacobians.push_back(jacobian); + if (debug) jacobian->print("Existing JacobianFactor: "); + } else { + H::shared_ptr hessian(boost::dynamic_pointer_cast(factor)); + if (!hessian) throw std::invalid_argument( + "convertToJacobians: factor is neither a JacobianFactor nor a HessianFactor."); + J::shared_ptr converted(new J(*hessian)); + if (debug) { + cout << "Converted HessianFactor to JacobianFactor:\n"; + hessian->print("HessianFactor: "); + converted->print("JacobianFactor: "); + if (!assert_equal(*hessian, HessianFactor(*converted), 1e-3)) throw runtime_error( + "convertToJacobians: Conversion between Jacobian and Hessian incorrect"); + } + jacobians.push_back(converted); + } + } + return jacobians; + } + + /* ************************************************************************* */ + GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph< + GaussianFactor>& factors, size_t nrFrontals) { + + const bool debug = ISDEBUG("EliminateQR"); + + // Convert all factors to the appropriate type and call the type-specific EliminateGaussian. + if (debug) cout << "Using QR" << endl; + + gttic(convert_to_Jacobian); + FactorGraph jacobians = convertToJacobians(factors); + gttoc(convert_to_Jacobian); + + gttic(Jacobian_EliminateGaussian); + GaussianConditional::shared_ptr conditional; + GaussianFactor::shared_ptr factor; + boost::tie(conditional, factor) = EliminateJacobians(jacobians, nrFrontals); + gttoc(Jacobian_EliminateGaussian); + + return make_pair(conditional, factor); + } // \EliminateQR + + /* ************************************************************************* */ + bool hasConstraints(const FactorGraph& factors) { + typedef JacobianFactor J; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); + if (jacobian && jacobian->get_model()->isConstrained()) { + return true; + } + } + return false; + } + + /* ************************************************************************* */ + GaussianFactorGraph::EliminationResult EliminatePreferCholesky( + const FactorGraph& factors, size_t nrFrontals) { + + // If any JacobianFactors have constrained noise models, we have to convert + // all factors to JacobianFactors. Otherwise, we can convert all factors + // to HessianFactors. This is because QR can handle constrained noise + // models but Cholesky cannot. + if (hasConstraints(factors)) + return EliminateQR(factors, nrFrontals); + else { + GaussianFactorGraph::EliminationResult ret; + gttic(EliminateCholesky); + ret = EliminateCholesky(factors, nrFrontals); + gttoc(EliminateCholesky); + return ret; + } + + } // \EliminatePreferCholesky + + + + /* ************************************************************************* */ + static JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { + JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast(gf); + if( !result ) { + result = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + } + return result; + } + + /* ************************************************************************* */ + Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) { + Errors e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + e.push_back((*Ai)*x); + } + return e; + } + + /* ************************************************************************* */ + void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e) { + multiplyInPlace(fg,x,e.begin()); + } + + /* ************************************************************************* */ + void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) { + Errors::iterator ei = e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + *ei = (*Ai)*x; + ei++; + } + } + + /* ************************************************************************* */ + // x += alpha*A'*e + void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) { + // For each factor add the gradient contribution + Errors::const_iterator ei = e.begin(); + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + Ai->transposeMultiplyAdd(alpha,*(ei++),x); + } + } + + /* ************************************************************************* */ + VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0) { + // It is crucial for performance to make a zero-valued clone of x + VectorValues g = VectorValues::Zero(x0); + Errors e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + e.push_back(Ai->error_vector(x0)); + } + transposeMultiplyAdd(fg, 1.0, e, g); + return g; + } + + /* ************************************************************************* */ + void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g) { + // Zero-out the gradient + g.setZero(); + Errors e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + e.push_back(-Ai->getb()); + } + transposeMultiplyAdd(fg, 1.0, e, g); + } + + /* ************************************************************************* */ + void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { + Index i = 0 ; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + r[i] = Ai->getb(); + i++; + } + VectorValues Ax = VectorValues::SameStructure(r); + multiply(fg,x,Ax); + axpy(-1.0,Ax,r); + } + + /* ************************************************************************* */ + void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { + r.setZero(); + Index i = 0; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + Vector &y = r[i]; + for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { + y += Ai->getA(j) * x[*j]; + } + ++i; + } + } + + /* ************************************************************************* */ + void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) { + x.setZero(); + Index i = 0; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { + x[*j] += Ai->getA(j).transpose() * r[i]; + } + ++i; + } + } + + /* ************************************************************************* */ + boost::shared_ptr gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x) { + boost::shared_ptr e(new Errors); + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + e->push_back(Ai->error_vector(x)); + } + return e; + } + +} // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraphUnordered.h b/gtsam/linear/GaussianFactorGraphUnordered.h new file mode 100644 index 000000000..761d684eb --- /dev/null +++ b/gtsam/linear/GaussianFactorGraphUnordered.h @@ -0,0 +1,384 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GaussianFactorGraph.h + * @brief Linear Factor Graph where all factors are Gaussians + * @author Kai Ni + * @author Christian Potthast + * @author Alireza Fathi + * @author Richard Roberts + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + // Forward declaration to use as default argument, documented declaration below. + GTSAM_EXPORT FactorGraph::EliminationResult + EliminateQR(const FactorGraph& factors, size_t nrFrontals); + + /** + * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. + * Factor == GaussianFactor + * VectorValues = A values structure of vectors + * Most of the time, linear factor graphs arise by linearizing a non-linear factor graph. + */ + class GaussianFactorGraph : public FactorGraph { + public: + + typedef boost::shared_ptr shared_ptr; + typedef FactorGraph Base; + + /** + * Default constructor + */ + GaussianFactorGraph() {} + + /** + * Constructor that receives a Chordal Bayes Net and returns a GaussianFactorGraph + */ + GTSAM_EXPORT GaussianFactorGraph(const GaussianBayesNet& CBN); + + /** + * Constructor that receives a BayesTree and returns a GaussianFactorGraph + */ + template + GaussianFactorGraph(const BayesTree& gbt) : Base(gbt) {} + + /** Constructor from a factor graph of GaussianFactor or a derived type */ + template + GaussianFactorGraph(const FactorGraph& fg) { + push_back(fg); + } + + /** Add a factor by value - makes a copy */ + void add(const GaussianFactor& factor) { + factors_.push_back(factor.clone()); + } + + /** Add a factor by pointer - stores pointer without copying the factor */ + void add(const sharedFactor& factor) { + factors_.push_back(factor); + } + + /** Add a null factor */ + void add(const Vector& b) { + add(JacobianFactor(b)); + } + + /** Add a unary factor */ + void add(Index key1, const Matrix& A1, + const Vector& b, const SharedDiagonal& model) { + add(JacobianFactor(key1,A1,b,model)); + } + + /** Add a binary factor */ + void add(Index key1, const Matrix& A1, + Index key2, const Matrix& A2, + const Vector& b, const SharedDiagonal& model) { + add(JacobianFactor(key1,A1,key2,A2,b,model)); + } + + /** Add a ternary factor */ + void add(Index key1, const Matrix& A1, + Index key2, const Matrix& A2, + Index key3, const Matrix& A3, + const Vector& b, const SharedDiagonal& model) { + add(JacobianFactor(key1,A1,key2,A2,key3,A3,b,model)); + } + + /** Add an n-ary factor */ + void add(const std::vector > &terms, + const Vector &b, const SharedDiagonal& model) { + add(JacobianFactor(terms,b,model)); + } + + /** + * Return the set of variables involved in the factors (computes a set + * union). + */ + typedef FastSet Keys; + GTSAM_EXPORT Keys keys() const; + + + /** Eliminate the first \c n frontal variables, returning the resulting + * conditional and remaining factor graph - this is very inefficient for + * eliminating all variables, to do that use EliminationTree or + * JunctionTree. Note that this version simply calls + * FactorGraph::eliminateFrontals with EliminateQR as the + * eliminate function argument. + */ + std::pair eliminateFrontals(size_t nFrontals, const Eliminate& function = EliminateQR) const { + return Base::eliminateFrontals(nFrontals, function); } + + /** Factor the factor graph into a conditional and a remaining factor graph. + * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out + * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where + * \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is + * a probability density or likelihood, the factorization produces a + * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$. + * + * For efficiency, this function treats the variables to eliminate + * \c variables as fully-connected, so produces a dense (fully-connected) + * conditional on all of the variables in \c variables, instead of a sparse + * BayesNet. If the variables are not fully-connected, it is more efficient + * to sequentially factorize multiple times. + * Note that this version simply calls + * FactorGraph::eliminate with EliminateQR as the eliminate + * function argument. + */ + std::pair eliminate(const std::vector& variables, const Eliminate& function = EliminateQR) const { + return Base::eliminate(variables, function); } + + /** Eliminate a single variable, by calling GaussianFactorGraph::eliminate. */ + std::pair eliminateOne(Index variable, const Eliminate& function = EliminateQR) const { + return Base::eliminateOne(variable, function); } + + /** Permute the variables in the factors */ + GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); + + /** Apply a reduction, which is a remapping of variable indices. */ + GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); + + /** unnormalized error */ + double error(const VectorValues& x) const { + double total_error = 0.; + BOOST_FOREACH(const sharedFactor& factor, *this) + total_error += factor->error(x); + return total_error; + } + + /** Unnormalized probability. O(n) */ + double probPrime(const VectorValues& c) const { + 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 + */ + GTSAM_EXPORT static GaussianFactorGraph combine2(const GaussianFactorGraph& lfg1, + const GaussianFactorGraph& lfg2); + + /** + * combine two factor graphs + * @param *lfg Linear factor graph + */ + GTSAM_EXPORT void combine(const GaussianFactorGraph &lfg); + + /** + * Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix, + * where i(k) and j(k) are the base 0 row and column indices, s(k) a double. + * The standard deviations are baked into A and b + */ + GTSAM_EXPORT std::vector > sparseJacobian() const; + + /** + * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries + * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. + * The standard deviations are baked into A and b + */ + GTSAM_EXPORT Matrix sparseJacobian_() const; + + /** + * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ + * Jacobian matrix, augmented with b with the noise models baked + * into A and b. The negative log-likelihood is + * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also + * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. + */ + GTSAM_EXPORT Matrix augmentedJacobian() const; + + /** + * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$, + * with the noise models baked into A and b. The negative log-likelihood + * is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also + * GaussianFactorGraph::augmentedJacobian and + * GaussianFactorGraph::sparseJacobian. + */ + GTSAM_EXPORT std::pair jacobian() const; + + /** + * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian + * matrix, augmented with the information vector \f$ \eta \f$. The + * augmented Hessian is + \f[ \left[ \begin{array}{ccc} + \Lambda & \eta \\ + \eta^T & c + \end{array} \right] \f] + and the negative log-likelihood is + \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$. + */ + GTSAM_EXPORT Matrix augmentedHessian() const; + + /** + * Return the dense Hessian \f$ \Lambda \f$ and information vector + * \f$ \eta \f$, with the noise models baked in. The negative log-likelihood + * is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also + * GaussianFactorGraph::augmentedHessian. + */ + GTSAM_EXPORT std::pair hessian() const; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } + + }; + + /** + * Combine and eliminate several factors. + * \addtogroup LinearSolving + */ + GTSAM_EXPORT JacobianFactor::shared_ptr CombineJacobians( + const FactorGraph& factors, + const VariableSlots& variableSlots); + + /** + * Evaluates whether linear factors have any constrained noise models + * @return true if any factor is constrained. + */ + GTSAM_EXPORT bool hasConstraints(const FactorGraph& factors); + + /** + * Densely combine and partially eliminate JacobianFactors to produce a + * single conditional with nrFrontals frontal variables and a remaining + * factor. + * Variables are eliminated in the natural order of the variable indices of in the factors. + * @param factors Factors to combine and eliminate + * @param nrFrontals Number of frontal variables to eliminate. + * @return The conditional and remaining factor + + * \addtogroup LinearSolving + */ + GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateJacobians(const FactorGraph< + JacobianFactor>& factors, size_t nrFrontals = 1); + + /** + * Densely partially eliminate with QR factorization. HessianFactors are + * first factored with Cholesky decomposition to produce JacobianFactors, + * by calling the conversion constructor JacobianFactor(const HessianFactor&). + * Variables are eliminated in the natural order of the variable indices of in + * the factors. + * @param factors Factors to combine and eliminate + * @param nrFrontals Number of frontal variables to eliminate. + * @return The conditional and remaining factor + + * \addtogroup LinearSolving + */ + GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph< + GaussianFactor>& factors, size_t nrFrontals = 1); + + /** + * Densely partially eliminate with Cholesky factorization. JacobianFactors + * are left-multiplied with their transpose to form the Hessian using the + * conversion constructor HessianFactor(const JacobianFactor&). + * + * If any factors contain constrained noise models (any sigmas equal to + * zero), QR factorization will be performed instead, because our current + * implementation cannot handle constrained noise models in Cholesky + * factorization. EliminateCholesky(), on the other hand, will fail if any + * factors contain constrained noise models. + * + * Variables are eliminated in the natural order of the variable indices of in + * the factors. + * @param factors Factors to combine and eliminate + * @param nrFrontals Number of frontal variables to eliminate. + * @return The conditional and remaining factor + + * \addtogroup LinearSolving + */ + GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminatePreferCholesky(const FactorGraph< + GaussianFactor>& factors, size_t nrFrontals = 1); + + /** + * Densely partially eliminate with Cholesky factorization. JacobianFactors + * are left-multiplied with their transpose to form the Hessian using the + * conversion constructor HessianFactor(const JacobianFactor&). + * + * If any factors contain constrained noise models, this function will fail + * because our current implementation cannot handle constrained noise models + * in Cholesky factorization. The function EliminatePreferCholesky() + * automatically does QR instead when this is the case. + * + * Variables are eliminated in the natural order of the variable indices of in + * the factors. + * @param factors Factors to combine and eliminate + * @param nrFrontals Number of frontal variables to eliminate. + * @return The conditional and remaining factor + + * \addtogroup LinearSolving + */ + GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< + GaussianFactor>& factors, size_t nrFrontals = 1); + + /****** Linear Algebra Opeations ******/ + + /** return A*x */ + GTSAM_EXPORT Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x); + + /** In-place version e <- A*x that overwrites e. */ + GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e); + + /** In-place version e <- A*x that takes an iterator. */ + GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e); + + /** x += alpha*A'*e */ + GTSAM_EXPORT void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& 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 VectorValues + */ + GTSAM_EXPORT VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& 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 VectorValues to store the gradient, which must be preallocated, see allocateVectorValues + * @return The gradient as a VectorValues + */ + GTSAM_EXPORT void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g); + + /* matrix-vector operations */ + GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); + GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); + GTSAM_EXPORT void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x); + + /** shared pointer version + * \todo Make this a member function - affects SubgraphPreconditioner */ + GTSAM_EXPORT boost::shared_ptr gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x); + + /** return A*x-b + * \todo Make this a member function - affects SubgraphPreconditioner */ + inline Errors gaussianErrors(const GaussianFactorGraph& fg, const VectorValues& x) { + return *gaussianErrors_(fg, x); } + +} // namespace gtsam diff --git a/gtsam/linear/GaussianFactorUnordered.h b/gtsam/linear/GaussianFactorUnordered.h new file mode 100644 index 000000000..5011b025b --- /dev/null +++ b/gtsam/linear/GaussianFactorUnordered.h @@ -0,0 +1,98 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GaussianFactor.h + * @brief A factor with a quadratic error function - a Gaussian + * @brief GaussianFactor + * @author Richard Roberts, Christian Potthast + */ + +// \callgraph + +#pragma once + +#include +#include + +namespace gtsam { + + // Forward declarations + class VectorValues; + + /** + * An abstract virtual base class for JacobianFactor and HessianFactor. A GaussianFactor has a + * quadratic error function. GaussianFactor is non-mutable (all methods const!). The factor value + * is exp(-0.5*||Ax-b||^2) */ + class GTSAM_EXPORT GaussianFactorUnordered : public FactorUnordered + { + public: + typedef GaussianFactorUnordered This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef FactorUnordered Base; ///< Our base class + + /** Default constructor creates empty factor */ + GaussianFactorUnordered() {} + + /** Construct from container of keys. This constructor is used internally from derived factor + * constructors, either from a container of keys or from a boost::assign::list_of. */ + template + GaussianFactorUnordered(const CONTAINER& keys) : Base(keys) {} + + // Implementing Testable interface + virtual void print(const std::string& s = "", + const IndexFormatter& formatter = DefaultIndexFormatter) const = 0; + + /** Equals for testable */ + virtual bool equals(const GaussianFactorUnordered& lf, double tol = 1e-9) const = 0; + + /** Print for testable */ + virtual double error(const VectorValues& c) const = 0; /** 0.5*(A*x-b)'*D*(A*x-b) */ + + /** Return the dimension of the variable pointed to by the given key iterator */ + virtual size_t getDim(const_iterator variable) const = 0; + + /** Return the augmented information matrix represented by this GaussianFactorUnordered. + * The augmented information matrix contains the information matrix with an + * additional column holding the information vector, and an additional row + * holding the transpose of the information vector. The lower-right entry + * contains the constant error term (when \f$ \delta x = 0 \f$). The + * augmented information matrix is described in more detail in HessianFactor, + * which in fact stores an augmented information matrix. + */ + virtual Matrix augmentedInformation() const = 0; + + /** Return the non-augmented information matrix represented by this + * GaussianFactorUnordered. + */ + virtual Matrix information() const = 0; + + /** Clone a factor (make a deep copy) */ + virtual GaussianFactorUnordered::shared_ptr clone() const = 0; + + /** + * Construct the corresponding anti-factor to negate information + * stored stored in this factor. + * @return a HessianFactor with negated Hessian matrices + */ + //virtual GaussianFactorUnordered::shared_ptr negate() const = 0; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } + + }; // GaussianFactorUnordered + +} // namespace gtsam diff --git a/gtsam/linear/HessianFactorUnordered.cpp b/gtsam/linear/HessianFactorUnordered.cpp new file mode 100644 index 000000000..9d67cac0d --- /dev/null +++ b/gtsam/linear/HessianFactorUnordered.cpp @@ -0,0 +1,560 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file HessianFactor.cpp + * @author Richard Roberts + * @date Dec 8, 2010 + */ + +#include + +#include +#include +#include +#include +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#endif +#include +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +string SlotEntry::toString() const { + ostringstream oss; + oss << "SlotEntry: slot=" << slot << ", dim=" << dimension; + return oss.str(); +} + +/* ************************************************************************* */ +Scatter::Scatter(const FactorGraph& gfg) { + // First do the set union. + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) { + if(factor) { + for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { + this->insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable)))); + } + } + } + + // Next fill in the slot indices (we can only get these after doing the set + // union. + size_t slot = 0; + BOOST_FOREACH(value_type& var_slot, *this) { + var_slot.second.slot = (slot ++); + } +} + +/* ************************************************************************* */ +void HessianFactor::assertInvariants() const { + GaussianFactor::assertInvariants(); // The base class checks for unique keys +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(const HessianFactor& gf) : + GaussianFactor(gf), info_(matrix_) { + info_.assignNoalias(gf.info_); + assertInvariants(); +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor() : info_(matrix_) { + // The empty HessianFactor has only a constant error term of zero + FastVector dims; + dims.push_back(1); + info_.resize(dims.begin(), dims.end(), false); + info_(0,0)(0,0) = 0.0; + assertInvariants(); +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(Index j, const Matrix& G, const Vector& g, double f) : + GaussianFactor(j), info_(matrix_) { + if(G.rows() != G.cols() || G.rows() != g.size()) + throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + size_t dims[] = { G.rows(), 1 }; + InfoMatrix fullMatrix(G.rows() + 1, G.rows() + 1); + BlockInfo infoMatrix(fullMatrix, dims, dims+2); + infoMatrix(0,0) = G; + infoMatrix.column(0,1,0) = g; + infoMatrix(1,1)(0,0) = f; + infoMatrix.swap(info_); + assertInvariants(); +} + +/* ************************************************************************* */ +// error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu) +// where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g +HessianFactor::HessianFactor(Index j, const Vector& mu, const Matrix& Sigma) : + GaussianFactor(j), info_(matrix_) { + if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( + "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + Matrix G = inverse(Sigma); + Vector g = G * mu; + double f = dot(mu, g); + size_t dims[] = { G.rows(), 1 }; + InfoMatrix fullMatrix(G.rows() + 1, G.rows() + 1); + BlockInfo infoMatrix(fullMatrix, dims, dims + 2); + infoMatrix(0, 0) = G; + infoMatrix.column(0, 1, 0) = g; + infoMatrix(1, 1)(0, 0) = f; + infoMatrix.swap(info_); + assertInvariants(); +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(Index j1, Index j2, + const Matrix& G11, const Matrix& G12, const Vector& g1, + const Matrix& G22, const Vector& g2, double f) : + GaussianFactor(j1, j2), info_(matrix_) { + if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != g1.size() || + G22.cols() != G12.cols() || G22.cols() != g2.size()) + throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + size_t dims[] = { G11.rows(), G22.rows(), 1 }; + InfoMatrix fullMatrix(G11.rows() + G22.rows() + 1, G11.rows() + G22.rows() + 1); + BlockInfo infoMatrix(fullMatrix, dims, dims+3); + infoMatrix(0,0) = G11; + infoMatrix(0,1) = G12; + infoMatrix.column(0,2,0) = g1; + infoMatrix(1,1) = G22; + infoMatrix.column(1,2,0) = g2; + infoMatrix(2,2)(0,0) = f; + infoMatrix.swap(info_); + assertInvariants(); +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(Index j1, Index j2, Index j3, + const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, + const Matrix& G22, const Matrix& G23, const Vector& g2, + const Matrix& G33, const Vector& g3, double f) : + GaussianFactor(j1, j2, j3), info_(matrix_) { + if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() || + G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size()) + throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + size_t dims[] = { G11.rows(), G22.rows(), G33.rows(), 1 }; + InfoMatrix fullMatrix(G11.rows() + G22.rows() + G33.rows() + 1, G11.rows() + G22.rows() + G33.rows() + 1); + BlockInfo infoMatrix(fullMatrix, dims, dims+4); + infoMatrix(0,0) = G11; + infoMatrix(0,1) = G12; + infoMatrix(0,2) = G13; + infoMatrix.column(0,3,0) = g1; + infoMatrix(1,1) = G22; + infoMatrix(1,2) = G23; + infoMatrix.column(1,3,0) = g2; + infoMatrix(2,2) = G33; + infoMatrix.column(2,3,0) = g3; + infoMatrix(3,3)(0,0) = f; + infoMatrix.swap(info_); + assertInvariants(); +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, + const std::vector& gs, double f) : GaussianFactor(js), info_(matrix_) { + + // Get the number of variables + size_t variable_count = js.size(); + + // Verify the provided number of entries in the vectors are consistent + if(gs.size() != variable_count || Gs.size() != (variable_count*(variable_count+1))/2) + throw invalid_argument("Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \ + in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2"); + + // Verify the dimensions of each provided matrix are consistent + // Note: equations for calculating the indices derived from the "sum of an arithmetic sequence" formula + for(size_t i = 0; i < variable_count; ++i){ + int block_size = gs[i].size(); + // Check rows + for(size_t j = 0; j < variable_count-i; ++j){ + size_t index = i*(2*variable_count - i + 1)/2 + j; + if(Gs[index].rows() != block_size){ + throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + } + } + // Check cols + for(size_t j = 0; j <= i; ++j){ + size_t index = j*(2*variable_count - j + 1)/2 + (i-j); + if(Gs[index].cols() != block_size){ + throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + } + } + } + + // Create the dims vector + size_t* dims = (size_t*)alloca(sizeof(size_t)*(variable_count+1)); // FIXME: alloca is bad, just ask Google. + size_t total_size = 0; + for(unsigned int i = 0; i < variable_count; ++i){ + dims[i] = gs[i].size(); + total_size += gs[i].size(); + } + dims[variable_count] = 1; + total_size += 1; + + // Fill in the internal matrix with the supplied blocks + InfoMatrix fullMatrix(total_size, total_size); + BlockInfo infoMatrix(fullMatrix, dims, dims+variable_count+1); + size_t index = 0; + for(size_t i = 0; i < variable_count; ++i){ + for(size_t j = i; j < variable_count; ++j){ + infoMatrix(i,j) = Gs[index++]; + } + infoMatrix.column(i,variable_count,0) = gs[i]; + } + infoMatrix(variable_count,variable_count)(0,0) = f; + + // update the BlockView variable + infoMatrix.swap(info_); + + assertInvariants(); +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(const GaussianConditional& cg) : GaussianFactor(cg), info_(matrix_) { + JacobianFactor jf(cg); + info_.copyStructureFrom(jf.Ab_); + matrix_.noalias() = jf.matrix_.transpose() * jf.matrix_; + assertInvariants(); +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(const GaussianFactor& gf) : info_(matrix_) { + // Copy the variable indices + (GaussianFactor&)(*this) = gf; + // Copy the matrix data depending on what type of factor we're copying from + if(dynamic_cast(&gf)) { + const JacobianFactor& jf(static_cast(gf)); + if(jf.model_->isConstrained()) + throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); + else { + Vector invsigmas = jf.model_->invsigmas().cwiseProduct(jf.model_->invsigmas()); + info_.copyStructureFrom(jf.Ab_); + BlockInfo::constBlock A = jf.Ab_.full(); + matrix_.noalias() = A.transpose() * invsigmas.asDiagonal() * A; + } + } else if(dynamic_cast(&gf)) { + const HessianFactor& hf(static_cast(gf)); + info_.assignNoalias(hf.info_); + } else + throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); + assertInvariants(); +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(const FactorGraph& factors) : info_(matrix_) +{ + Scatter scatter(factors); + + // Pull out keys and dimensions + gttic(keys); + vector dimensions(scatter.size() + 1); + BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { + dimensions[var_slot.second.slot] = var_slot.second.dimension; + } + // This is for the r.h.s. vector + dimensions.back() = 1; + gttoc(keys); + + const bool debug = ISDEBUG("EliminateCholesky"); + // Form Ab' * Ab + gttic(allocate); + info_.resize(dimensions.begin(), dimensions.end(), false); + // Fill in keys + keys_.resize(scatter.size()); + std::transform(scatter.begin(), scatter.end(), keys_.begin(), boost::bind(&Scatter::value_type::first, ::_1)); + gttoc(allocate); + gttic(zero); + matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols()); + gttoc(zero); + gttic(update); + if (debug) cout << "Combining " << factors.size() << " factors" << endl; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) + { + if(factor) { + if(shared_ptr hessian = boost::dynamic_pointer_cast(factor)) + updateATA(*hessian, scatter); + else if(JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast(factor)) + updateATA(*jacobianFactor, scatter); + else + throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); + } + } + gttoc(update); + + if (debug) gtsam::print(matrix_, "Ab' * Ab: "); + + assertInvariants(); +} + +/* ************************************************************************* */ +HessianFactor& HessianFactor::operator=(const HessianFactor& rhs) { + this->Base::operator=(rhs); // Copy keys + info_.assignNoalias(rhs.info_); // Copy matrix and block structure + return *this; +} + +/* ************************************************************************* */ +void HessianFactor::print(const std::string& s, const IndexFormatter& formatter) const { + cout << s << "\n"; + cout << " keys: "; + for(const_iterator key=this->begin(); key!=this->end(); ++key) + cout << formatter(*key) << "(" << this->getDim(key) << ") "; + cout << "\n"; + gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView()), "Ab^T * Ab: "); +} + +/* ************************************************************************* */ +bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { + if(!dynamic_cast(&lf)) + return false; + else { + Matrix thisMatrix = this->info_.full().selfadjointView(); + thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; + Matrix rhsMatrix = static_cast(lf).info_.full().selfadjointView(); + rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; + return equal_with_abs_tol(thisMatrix, rhsMatrix, tol); + } +} + +/* ************************************************************************* */ +Matrix HessianFactor::augmentedInformation() const { + return info_.full().selfadjointView(); +} + +/* ************************************************************************* */ +Matrix HessianFactor::information() const { + return info_.range(0, this->size(), 0, this->size()).selfadjointView(); +} + +/* ************************************************************************* */ +double HessianFactor::error(const VectorValues& c) const { + // error 0.5*(f - 2*x'*g + x'*G*x) + const double f = constantTerm(); + double xtg = 0, xGx = 0; + // extract the relevant subset of the VectorValues + // NOTE may not be as efficient + const Vector x = c.vector(this->keys()); + xtg = x.dot(linearTerm()); + xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; + return 0.5 * (f - 2.0 * xtg + xGx); +} + +/* ************************************************************************* */ +void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) { + + // This function updates 'combined' with the information in 'update'. + // 'scatter' maps variables in the update factor to slots in the combined + // factor. + + const bool debug = ISDEBUG("updateATA"); + + // First build an array of slots + gttic(slots); + size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. + size_t slot = 0; + BOOST_FOREACH(Index j, update) { + slots[slot] = scatter.find(j)->second.slot; + ++ slot; + } + gttoc(slots); + + if(debug) { + this->print("Updating this: "); + update.print("with (Hessian): "); + } + + // Apply updates to the upper triangle + gttic(update); + for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; + for(size_t j1=0; j1<=j2; ++j1) { + size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; + if(slot2 > slot1) { + if(debug) + cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; + matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() += + update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()); + } else if(slot1 > slot2) { + if(debug) + cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl; + matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() += + update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()).transpose(); + } else { + if(debug) + cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; + matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView() += + update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()); + } + if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n"; + if(debug) this->print(); + } + } + gttoc(update); +} + +/* ************************************************************************* */ +void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter) { + + // This function updates 'combined' with the information in 'update'. + // 'scatter' maps variables in the update factor to slots in the combined + // factor. + + const bool debug = ISDEBUG("updateATA"); + + // First build an array of slots + gttic(slots); + size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. + size_t slot = 0; + BOOST_FOREACH(Index j, update) { + slots[slot] = scatter.find(j)->second.slot; + ++ slot; + } + gttoc(slots); + + gttic(form_ATA); + if(update.model_->isConstrained()) + throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model"); + + if(debug) { + this->print("Updating this: "); + update.print("with (Jacobian): "); + } + + typedef Eigen::Block BlockUpdateMatrix; + BlockUpdateMatrix updateA(update.matrix_.block( + update.Ab_.rowStart(),update.Ab_.offset(0), update.Ab_.full().rows(), update.Ab_.full().cols())); + if (debug) cout << "updateA: \n" << updateA << endl; + + Matrix updateInform; + if(boost::dynamic_pointer_cast(update.model_)) { + updateInform.noalias() = updateA.transpose() * updateA; + } else { + noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast(update.model_)); + if(diagonal) { + Vector invsigmas2 = update.model_->invsigmas().cwiseProduct(update.model_->invsigmas()); + updateInform.noalias() = updateA.transpose() * invsigmas2.asDiagonal() * updateA; + } else + throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal"); + } + if (debug) cout << "updateInform: \n" << updateInform << endl; + gttoc(form_ATA); + + // Apply updates to the upper triangle + gttic(update); + for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; + for(size_t j1=0; j1<=j2; ++j1) { + size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; + size_t off0 = update.Ab_.offset(0); + if(slot2 > slot1) { + if(debug) + cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; + matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() += + updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()); + } else if(slot1 > slot2) { + if(debug) + cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl; + matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() += + updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()).transpose(); + } else { + if(debug) + cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; + matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView() += + updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()); + } + if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n"; + if(debug) this->print(); + } + } + gttoc(update); +} + +/* ************************************************************************* */ +void HessianFactor::partialCholesky(size_t nrFrontals) { + if(!choleskyPartial(matrix_, info_.offset(nrFrontals))) + throw IndeterminantLinearSystemException(this->keys().front()); +} + +/* ************************************************************************* */ +GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFrontals) { + + static const bool debug = false; + + // Extract conditionals + gttic(extract_conditionals); + GaussianConditional::shared_ptr conditional(new GaussianConditional()); + typedef VerticalBlockView BlockAb; + BlockAb Ab(matrix_, info_); + + size_t varDim = info_.offset(nrFrontals); + Ab.rowEnd() = Ab.rowStart() + varDim; + + // Create one big conditionals with many frontal variables. + gttic(construct_cond); + Vector sigmas = Vector::Ones(varDim); + conditional = boost::make_shared(keys_.begin(), keys_.end(), nrFrontals, Ab, sigmas); + gttoc(construct_cond); + if(debug) conditional->print("Extracted conditional: "); + + gttoc(extract_conditionals); + + // Take lower-right block of Ab_ to get the new factor + gttic(remaining_factor); + info_.blockStart() = nrFrontals; + // Assign the keys + vector remainingKeys(keys_.size() - nrFrontals); + remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end()); + keys_.swap(remainingKeys); + gttoc(remaining_factor); + + return conditional; +} + +/* ************************************************************************* */ +GaussianFactor::shared_ptr HessianFactor::negate() const { + // Copy Hessian Blocks from Hessian factor and invert + std::vector js; + std::vector Gs; + std::vector gs; + double f; + js.insert(js.end(), begin(), end()); + for(size_t i = 0; i < js.size(); ++i){ + for(size_t j = i; j < js.size(); ++j){ + Gs.push_back( -info(begin()+i, begin()+j) ); + } + gs.push_back( -linearTerm(begin()+i) ); + } + f = -constantTerm(); + + // Create the Anti-Hessian Factor from the negated blocks + return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); +} + +} // gtsam diff --git a/gtsam/linear/HessianFactorUnordered.h b/gtsam/linear/HessianFactorUnordered.h new file mode 100644 index 000000000..0c307aa74 --- /dev/null +++ b/gtsam/linear/HessianFactorUnordered.h @@ -0,0 +1,377 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file HessianFactor.h + * @brief Contains the HessianFactor class, a general quadratic factor + * @author Richard Roberts + * @date Dec 8, 2010 + */ + +#pragma once + +#include +#include +#include + +// Forward declarations for friend unit tests +class HessianFactorConversionConstructorTest; +class HessianFactorConstructor1Test; +class HessianFactorConstructor1bTest; +class HessianFactorcombineTest; + + +namespace gtsam { + + // Forward declarations + class JacobianFactor; + class GaussianConditional; + template class BayesNet; + + // Definition of Scatter, which is an intermediate data structure used when + // building a HessianFactor incrementally, to get the keys in the right + // order. + // The "scatter" is a map from global variable indices to slot indices in the + // union of involved variables. We also include the dimensionality of the + // variable. + struct GTSAM_EXPORT SlotEntry { + size_t slot; + size_t dimension; + SlotEntry(size_t _slot, size_t _dimension) + : slot(_slot), dimension(_dimension) {} + std::string toString() const; + }; + class Scatter : public FastMap { + public: + Scatter() {} + Scatter(const FactorGraph& gfg); + }; + + /** + * @brief A Gaussian factor using the canonical parameters (information form) + * + * HessianFactor implements a general quadratic factor of the form + * \f[ E(x) = 0.5 x^T G x - x^T g + 0.5 f \f] + * that stores the matrix \f$ G \f$, the vector \f$ g \f$, and the constant term \f$ f \f$. + * + * When \f$ G \f$ is positive semidefinite, this factor represents a Gaussian, + * in which case \f$ G \f$ is the information matrix \f$ \Lambda \f$, + * \f$ g \f$ is the information vector \f$ \eta \f$, and \f$ f \f$ is the residual + * sum-square-error at the mean, when \f$ x = \mu \f$. + * + * Indeed, the negative log-likelihood of a Gaussian is (up to a constant) + * @f$ E(x) = 0.5(x-\mu)^T P^{-1} (x-\mu) @f$ + * with @f$ \mu @f$ the mean and @f$ P @f$ the covariance matrix. Expanding the product we get + * @f[ + * E(x) = 0.5 x^T P^{-1} x - x^T P^{-1} \mu + 0.5 \mu^T P^{-1} \mu + * @f] + * We define the Information matrix (or Hessian) @f$ \Lambda = P^{-1} @f$ + * and the information vector @f$ \eta = P^{-1} \mu = \Lambda \mu @f$ + * to arrive at the canonical form of the Gaussian: + * @f[ + * E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu + * @f] + * + * This factor is one of the factors that can be in a GaussianFactorGraph. + * It may be returned from NonlinearFactor::linearize(), but is also + * used internally to store the Hessian during Cholesky elimination. + * + * This can represent a quadratic factor with characteristics that cannot be + * represented using a JacobianFactor (which has the form + * \f$ E(x) = \Vert Ax - b \Vert^2 \f$ and stores the Jacobian \f$ A \f$ + * and error vector \f$ b \f$, i.e. is a sum-of-squares factor). For example, + * a HessianFactor need not be positive semidefinite, it can be indefinite or + * even negative semidefinite. + * + * If a HessianFactor is indefinite or negative semi-definite, then in order + * for solving the linear system to be possible, + * the Hessian of the full system must be positive definite (i.e. when all + * small Hessians are combined, the result must be positive definite). If + * this is not the case, an error will occur during elimination. + * + * This class stores G, g, and f as an augmented matrix HessianFactor::matrix_. + * The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right + * triangle of G, the upper-right-most column of length n of HessianFactor::matrix_ + * stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e. + * \code + HessianFactor::matrix_ = [ G11 G12 G13 ... g1 + 0 G22 G23 ... g2 + 0 0 G33 ... g3 + : : : : + 0 0 0 ... f ] + \endcode + Blocks can be accessed as follows: + \code + G11 = info(begin(), begin()); + G12 = info(begin(), begin()+1); + G23 = info(begin()+1, begin()+2); + g2 = linearTerm(begin()+1); + f = constantTerm(); + ....... + \endcode + */ + class GTSAM_EXPORT HessianFactor : public GaussianFactor { + protected: + typedef Matrix InfoMatrix; ///< The full augmented Hessian + typedef SymmetricBlockView BlockInfo; ///< A blockwise view of the Hessian + typedef GaussianFactor Base; ///< Typedef to base class + + InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1] + BlockInfo info_; ///< The block view of the full information matrix. + + public: + + typedef boost::shared_ptr shared_ptr; ///< A shared_ptr to this + typedef BlockInfo::Block Block; ///< A block from the Hessian matrix + typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version) + typedef BlockInfo::Column Column; ///< A column containing the linear term h + typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version) + + /** Copy constructor */ + HessianFactor(const HessianFactor& gf); + + /** default constructor for I/O */ + HessianFactor(); + + /** Construct a unary factor. G is the quadratic term (Hessian matrix), g + * the linear term (a vector), and f the constant term. The quadratic + * error is: + * 0.5*(f - 2*x'*g + x'*G*x) + */ + HessianFactor(Index j, const Matrix& G, const Vector& g, double f); + + /** Construct a unary factor, given a mean and covariance matrix. + * error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) + */ + HessianFactor(Index j, const Vector& mu, const Matrix& Sigma); + + /** Construct a binary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + * JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f] + * HessianFactor error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx - x'*g + 0.5*f \f] + * So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have + \code + n1*n1 G11 = A1'*M*A1 + n1*n2 G12 = A1'*M*A2 + n2*n2 G22 = A2'*M*A2 + n1*1 g1 = A1'*M*b + n2*1 g2 = A2'*M*b + 1*1 f = b'*M*b + \endcode + */ + HessianFactor(Index j1, Index j2, + const Matrix& G11, const Matrix& G12, const Vector& g1, + const Matrix& G22, const Vector& g2, double f); + + /** Construct a ternary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + */ + HessianFactor(Index j1, Index j2, Index j3, + const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, + const Matrix& G22, const Matrix& G23, const Vector& g2, + const Matrix& G33, const Vector& g3, double f); + + /** Construct an n-way factor. Gs contains the upper-triangle blocks of the + * quadratic term (the Hessian matrix) provided in row-order, gs the pieces + * of the linear vector term, and f the constant term. + */ + HessianFactor(const std::vector& js, const std::vector& Gs, + const std::vector& gs, double f); + + /** Construct from Conditional Gaussian */ + explicit HessianFactor(const GaussianConditional& cg); + + /** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */ + explicit HessianFactor(const GaussianFactor& factor); + + /** Combine a set of factors into a single dense HessianFactor */ + HessianFactor(const FactorGraph& factors); + + /** Destructor */ + virtual ~HessianFactor() {} + + /** Aassignment operator */ + HessianFactor& operator=(const HessianFactor& rhs); + + /** Clone this JacobianFactor */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + shared_ptr(new HessianFactor(*this))); + } + + /** Print the factor for debugging and testing (implementing Testable) */ + virtual void print(const std::string& s = "", + const IndexFormatter& formatter = DefaultIndexFormatter) const; + + /** Compare to another factor for testing (implementing Testable) */ + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; + + /** Evaluate the factor error f(x), see above. */ + virtual double error(const VectorValues& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */ + + /** Return the dimension of the variable pointed to by the given key iterator + * todo: Remove this in favor of keeping track of dimensions with variables? + * @param variable An iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + */ + virtual size_t getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); } + + /** Return the number of columns and rows of the Hessian matrix */ + size_t rows() const { return info_.rows(); } + + /** + * Construct the corresponding anti-factor to negate information + * stored stored in this factor. + * @return a HessianFactor with negated Hessian matrices + */ + virtual GaussianFactor::shared_ptr negate() const; + + /** Return a view of the block at (j1,j2) of the upper-triangular part of the + * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation + * above to explain that only the upper-triangular part of the information matrix is stored + * and returned by this function. + * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return A view of the requested block, not a copy. + */ + constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); } + + /** Return a view of the block at (j1,j2) of the upper-triangular part of the + * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation + * above to explain that only the upper-triangular part of the information matrix is stored + * and returned by this function. + * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return A view of the requested block, not a copy. + */ + Block info(iterator j1, iterator j2) { return info_(j1-begin(), j2-begin()); } + + /** Return the upper-triangular part of the full *augmented* information matrix, + * as described above. See HessianFactor class documentation above to explain that only the + * upper-triangular part of the information matrix is stored and returned by this function. + */ + constBlock info() const { return info_.full(); } + + /** Return the upper-triangular part of the full *augmented* information matrix, + * as described above. See HessianFactor class documentation above to explain that only the + * upper-triangular part of the information matrix is stored and returned by this function. + */ + Block info() { return info_.full(); } + + /** Return the constant term \f$ f \f$ as described above + * @return The constant term \f$ f \f$ + */ + double constantTerm() const { return info_(this->size(), this->size())(0,0); } + + /** Return the constant term \f$ f \f$ as described above + * @return The constant term \f$ f \f$ + */ + double& constantTerm() { return info_(this->size(), this->size())(0,0); } + + /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. + * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return The linear term \f$ g \f$ */ + constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); } + + /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. + * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return The linear term \f$ g \f$ */ + Column linearTerm(iterator j) { return info_.column(j-begin(), size(), 0); } + + /** Return the complete linear term \f$ g \f$ as described above. + * @return The linear term \f$ g \f$ */ + constColumn linearTerm() const { return info_.rangeColumn(0, this->size(), this->size(), 0); } + + /** Return the complete linear term \f$ g \f$ as described above. + * @return The linear term \f$ g \f$ */ + Column linearTerm() { return info_.rangeColumn(0, this->size(), this->size(), 0); } + + /** Return the augmented information matrix represented by this GaussianFactor. + * The augmented information matrix contains the information matrix with an + * additional column holding the information vector, and an additional row + * holding the transpose of the information vector. The lower-right entry + * contains the constant error term (when \f$ \delta x = 0 \f$). The + * augmented information matrix is described in more detail in HessianFactor, + * which in fact stores an augmented information matrix. + * + * For HessianFactor, this is the same as info() except that this function + * returns a complete symmetric matrix whereas info() returns a matrix where + * only the upper triangle is valid, but should be interpreted as symmetric. + * This is because info() returns only a reference to the internal + * representation of the augmented information matrix, which stores only the + * upper triangle. + */ + virtual Matrix augmentedInformation() const; + + /** Return the non-augmented information matrix represented by this + * GaussianFactor. + */ + virtual Matrix information() const; + + // Friend unit test classes + friend class ::HessianFactorConversionConstructorTest; + friend class ::HessianFactorConstructor1Test; + friend class ::HessianFactorConstructor1bTest; + friend class ::HessianFactorcombineTest; + + // Friend JacobianFactor for conversion + friend class JacobianFactor; + + // used in eliminateCholesky: + + /** + * Do Cholesky. Note that after this, the lower triangle still contains + * some untouched non-zeros that should be zero. We zero them while + * extracting submatrices in splitEliminatedFactor. Frank says :-( + */ + void partialCholesky(size_t nrFrontals); + + /** split partially eliminated factor */ + boost::shared_ptr splitEliminatedFactor(size_t nrFrontals); + + /** Update the factor by adding the information from the JacobianFactor + * (used internally during elimination). + * @param update The JacobianFactor containing the new information to add + * @param scatter A mapping from variable index to slot index in this HessianFactor + */ + void updateATA(const JacobianFactor& update, const Scatter& scatter); + + /** Update the factor by adding the information from the HessianFactor + * (used internally during elimination). + * @param update The HessianFactor containing the new information to add + * @param scatter A mapping from variable index to slot index in this HessianFactor + */ + void updateATA(const HessianFactor& update, const Scatter& scatter); + + /** assert invariants */ + void assertInvariants() const; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor); + ar & BOOST_SERIALIZATION_NVP(info_); + ar & BOOST_SERIALIZATION_NVP(matrix_); + } + }; + +} + diff --git a/gtsam/linear/JacobianFactorUnordered-inl.h b/gtsam/linear/JacobianFactorUnordered-inl.h new file mode 100644 index 000000000..43977a6c3 --- /dev/null +++ b/gtsam/linear/JacobianFactorUnordered-inl.h @@ -0,0 +1,76 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file JacobianFactor.h + * @author Richard Roberts + * @author Christian Potthast + * @author Frank Dellaert + * @date Dec 8, 2010 + */ +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + JacobianFactorUnordered::JacobianFactorUnordered(const TERMS&terms, const Vector &b, const SharedDiagonal& model) + { + fillTerms(terms, b, model); + } + + /* ************************************************************************* */ + template + void JacobianFactorUnordered::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel) + { + // Check noise model dimension + if(noiseModel && model->dim() != b.size()) + throw InvalidNoiseModel(b.size(), model->dim()); + + // Resize base class key vector + Base::keys_.resize(terms.size()); + + // Gather dimensions - uses boost range adaptors to take terms, extract .second which are the + // matrices, then extract the number of columns e.g. dimensions in each matrix. Then joins with + // a single '1' to add a dimension for the b vector. + using boost::adaptors::map_values; + using boost::adaptors::transformed; + using boost::join; + Ab_ = VerticalBlockMatrix(join(terms | map_values | transformed(Matrix::cols), cref_list_of<1>(1)), b.size()); + + // Check and add terms + typedef pair Term; + DenseIndex i = 0; // For block index + BOOST_FOREACH(const Term& term, terms) { + // Check block rows + if(term.second.rows() != b.size()) + throw InvalidMatrixBlock(b.size(), term.second.rows()); + // Assign key and matrix + Base::keys_[i] = term.first; + Ab_(i) = term.second; + // Increment block index + ++ i; + } + + // Assign RHS vector + getb() = b; + + // Assign noise model + model_ = noiseModel; + } + +} // gtsam + diff --git a/gtsam/linear/JacobianFactorUnordered.cpp b/gtsam/linear/JacobianFactorUnordered.cpp new file mode 100644 index 000000000..6a5535ef0 --- /dev/null +++ b/gtsam/linear/JacobianFactorUnordered.cpp @@ -0,0 +1,411 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file JacobianFactor.cpp + * @author Richard Roberts + * @author Christian Potthast + * @author Frank Dellaert + * @date Dec 8, 2010 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#endif +#include +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + +#include +#include +#include + +using namespace std; +using namespace boost::assign; + +namespace gtsam { + + /* ************************************************************************* */ + JacobianFactorUnordered::JacobianFactorUnordered(const GaussianFactorUnordered& gf) { + // Copy the matrix data depending on what type of factor we're copying from + if(const JacobianFactorUnordered* rhs = dynamic_cast(&gf)) + *this = JacobianFactorUnordered(*rhs); + //else if(const HessianFactor* rhs = dynamic_cast(&gf)) + // *this = JacobianFactorUnordered(*rhs); + else + throw std::invalid_argument("In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor"); + assertInvariants(); + } + + /* ************************************************************************* */ + JacobianFactorUnordered::JacobianFactorUnordered(const Vector& b_in) : + Ab_(cref_list_of<1>(1), b_in.size()) + { + getb() = b_in; + } + + /* ************************************************************************* */ + JacobianFactorUnordered::JacobianFactorUnordered(Index i1, const Matrix& A1, + const Vector& b, const SharedDiagonal& model) + { + fillTerms(cref_list_of<1>(make_pair(i1,A1)), b, model); + } + + /* ************************************************************************* */ + JacobianFactorUnordered::JacobianFactorUnordered(Index i1, const Matrix& A1, Index i2, const Matrix& A2, + const Vector& b, const SharedDiagonal& model) + { + fillTerms(cref_list_of<2> + (make_pair(i1,A1)) + (make_pair(i2,A2)), b, model); + } + + /* ************************************************************************* */ + JacobianFactorUnordered::JacobianFactorUnordered(Index i1, const Matrix& A1, Index i2, const Matrix& A2, + Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) + { + fillTerms(cref_list_of<3> + (make_pair(i1,A1)) + (make_pair(i2,A2)) + (make_pair(i3,A3)), b, model); + } + + /* ************************************************************************* */ + //JacobianFactorUnordered::JacobianFactorUnordered(const HessianFactorUnordered& factor) { + // keys_ = factor.keys_; + // Ab_.assignNoalias(factor.info_); + + // // Do Cholesky to get a Jacobian + // size_t maxrank; + // bool success; + // boost::tie(maxrank, success) = choleskyCareful(matrix_); + + // // Check for indefinite system + // if(!success) + // throw IndeterminantLinearSystemException(factor.keys().front()); + + // // Zero out lower triangle + // matrix_.topRows(maxrank).triangularView() = + // Matrix::Zero(maxrank, matrix_.cols()); + // // FIXME: replace with triangular system + // Ab_.rowEnd() = maxrank; + // model_ = noiseModel::Unit::Create(maxrank); + + // assertInvariants(); + //} + + /* ************************************************************************* */ + JacobianFactorUnordered::JacobianFactorUnordered(const GaussianFactorGraphUnordered& gfg) + { + // Cast or convert to Jacobians + std::vector jacobians; + jacobians.reserve(gfg.size()); + BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& factor, gfg) { + if(factor) { + if(JacobianFactorUnordered::shared_ptr jf = boost::dynamic_pointer_cast(factor)) + jacobians.push_back(jf); + else + jacobians.push_back(boost::make_shared(*factor)); + } + } + + *this = *CombineJacobians(jacobians, VariableSlots(jacobians)); + } + + /* ************************************************************************* */ + void JacobianFactorUnordered::print(const string& s, const KeyFormatter& formatter) const { + cout << s << "\n"; + if (empty()) { + cout << " empty, keys: "; + BOOST_FOREACH(const Key& key, keys()) { cout << formatter(key) << " "; } + cout << endl; + } else { + for(const_iterator key=begin(); key!=end(); ++key) + cout << boost::format("A[%1%]=\n")%formatter(*key) << getA(key) << endl; + cout << "b=" << getb() << endl; + model_->print("model"); + } + } + + /* ************************************************************************* */ + // Check if two linear factors are equal + bool JacobianFactorUnordered::equals(const GaussianFactorUnordered& f_, double tol) const { + if(!dynamic_cast(&f_)) + return false; + else { + const JacobianFactorUnordered& f(static_cast(f_)); + if (empty()) return (f.empty()); + if(keys()!=f.keys() /*|| !model_->equals(lf->model_, tol)*/) + return false; + + if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) + return false; + + constABlock Ab1(Ab_.range(0, Ab_.nBlocks())); + constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks())); + for(size_t row=0; row< (size_t) Ab1.rows(); ++row) + if(!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol) && + !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) + return false; + + return true; + } + } + + /* ************************************************************************* */ + Vector JacobianFactorUnordered::unweighted_error(const VectorValuesUnordered& c) const { + Vector e = -getb(); + if (empty()) return e; + for(size_t pos=0; poswhiten(-getb()); + return model_->whiten(unweighted_error(c)); + } + + /* ************************************************************************* */ + double JacobianFactorUnordered::error(const VectorValuesUnordered& c) const { + if (empty()) return 0; + Vector weighted = error_vector(c); + return 0.5 * weighted.dot(weighted); + } + + /* ************************************************************************* */ + Matrix JacobianFactorUnordered::augmentedInformation() const { + Matrix AbWhitened = Ab_.full(); + model_->WhitenInPlace(AbWhitened); + return AbWhitened.transpose() * AbWhitened; + } + + /* ************************************************************************* */ + Matrix JacobianFactorUnordered::information() const { + Matrix AWhitened = this->getA(); + model_->WhitenInPlace(AWhitened); + return AWhitened.transpose() * AWhitened; + } + + /* ************************************************************************* */ + Vector JacobianFactorUnordered::operator*(const VectorValuesUnordered& x) const { + Vector Ax = zero(Ab_.rows()); + if (empty()) return Ax; + + // Just iterate over all A matrices and multiply in correct config part + for(size_t pos=0; poswhiten(Ax); + } + + /* ************************************************************************* */ + void JacobianFactorUnordered::transposeMultiplyAdd(double alpha, const Vector& e, + VectorValuesUnordered& x) const { + Vector E = alpha * model_->whiten(e); + // Just iterate over all A matrices and insert Ai^e into VectorValues + for(size_t pos=0; pos JacobianFactorUnordered::matrix(bool weight) const { + Matrix A(Ab_.range(0, size())); + Vector b(getb()); + // divide in sigma so error is indeed 0.5*|Ax-b| + if (weight) model_->WhitenSystem(A,b); + return make_pair(A, b); + } + + /* ************************************************************************* */ + Matrix JacobianFactorUnordered::matrix_augmented(bool weight) const { + if (weight) { Matrix Ab(Ab_.range(0,Ab_.nBlocks())); model_->WhitenInPlace(Ab); return Ab; } + else return Ab_.range(0, Ab_.nBlocks()); + } + + /* ************************************************************************* */ + std::vector > + JacobianFactorUnordered::sparse(const std::vector& columnIndices) const { + + std::vector > entries; + + // iterate over all variables in the factor + for(const_iterator var=begin(); varWhiten(getA(var))); + // find first column index for this key + size_t column_start = columnIndices[*var]; + for (size_t i = 0; i < (size_t) whitenedA.rows(); i++) + for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) { + double s = whitenedA(i,j); + if (std::abs(s) > 1e-12) entries.push_back( + boost::make_tuple(i, column_start + j, s)); + } + } + + Vector whitenedb(model_->whiten(getb())); + size_t bcolumn = columnIndices.back(); + for (size_t i = 0; i < (size_t) whitenedb.size(); i++) + entries.push_back(boost::make_tuple(i, bcolumn, whitenedb(i))); + + // return the result + return entries; + } + + /* ************************************************************************* */ + JacobianFactorUnordered JacobianFactorUnordered::whiten() const { + JacobianFactorUnordered result(*this); + result.model_->WhitenInPlace(result.Ab_.matrix()); + result.model_ = noiseModel::Unit::Create(result.model_->dim()); + return result; + } + + /* ************************************************************************* */ + //GaussianFactorUnordered::shared_ptr JacobianFactorUnordered::negate() const { + // HessianFactor hessian(*this); + // return hessian.negate(); + //} + + /* ************************************************************************* */ + GaussianConditionalUnordered::shared_ptr JacobianFactorUnordered::eliminateFirst() { + return this->eliminate(1); + } + + /* ************************************************************************* */ + GaussianConditionalUnordered::shared_ptr JacobianFactorUnordered::splitConditional(size_t nrFrontals) { + assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0); + assert(size() >= nrFrontals); + assertInvariants(); + + const bool debug = ISDEBUG("JacobianFactor::splitConditional"); + + if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl; + if(debug) this->print("Splitting JacobianFactor: "); + + size_t frontalDim = Ab_.range(0,nrFrontals).cols(); + + // Check for singular factor + if(model_->dim() < frontalDim) + throw IndeterminantLinearSystemException(this->keys().front()); + + // Extract conditional + gttic(cond_Rd); + + // Restrict the matrix to be in the first nrFrontals variables + Ab_.rowEnd() = Ab_.rowStart() + frontalDim; + const Eigen::VectorBlock sigmas = model_->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart()); + GaussianConditional::shared_ptr conditional(new GaussianConditional(begin(), end(), nrFrontals, Ab_, sigmas)); + if(debug) conditional->print("Extracted conditional: "); + Ab_.rowStart() += frontalDim; + Ab_.firstBlock() += nrFrontals; + gttoc(cond_Rd); + + if(debug) conditional->print("Extracted conditional: "); + + gttic(remaining_factor); + // Take lower-right block of Ab to get the new factor + Ab_.rowEnd() = model_->dim(); + keys_.erase(begin(), begin() + nrFrontals); + // Set sigmas with the right model + if (model_->isConstrained()) + model_ = noiseModel::Constrained::MixedSigmas(sub(model_->sigmas(), frontalDim, model_->dim())); + else + model_ = noiseModel::Diagonal::Sigmas(sub(model_->sigmas(), frontalDim, model_->dim())); + if(debug) this->print("Eliminated factor: "); + assert(Ab_.rows() <= Ab_.cols()-1); + gttoc(remaining_factor); + + if(debug) print("Eliminated factor: "); + + assertInvariants(); + + return conditional; + } + + /* ************************************************************************* */ + GaussianConditionalUnordered::shared_ptr JacobianFactorUnordered::eliminate(size_t nrFrontals) { + + assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0); + assert(size() >= nrFrontals); + assertInvariants(); + + const bool debug = ISDEBUG("JacobianFactor::eliminate"); + + if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl; + if(debug) this->print("Eliminating JacobianFactor: "); + if(debug) gtsam::print(matrix_, "Augmented Ab: "); + + size_t frontalDim = Ab_.range(0,nrFrontals).cols(); + + if(debug) cout << "frontalDim = " << frontalDim << endl; + + // Use in-place QR dense Ab appropriate to NoiseModel + gttic(QR); + SharedDiagonal noiseModel = model_->QR(matrix_); + gttoc(QR); + + // Zero the lower-left triangle. todo: not all of these entries actually + // need to be zeroed if we are careful to start copying rows after the last + // structural zero. + if(matrix_.rows() > 0) + for(size_t j=0; j<(size_t) matrix_.cols(); ++j) + for(size_t i=j+1; idim(); ++i) + matrix_(i,j) = 0.0; + + if(debug) gtsam::print(matrix_, "QR result: "); + if(debug) noiseModel->print("QR result noise model: "); + + // Start of next part + model_ = noiseModel; + return splitConditional(nrFrontals); + } + + /* ************************************************************************* */ + //void JacobianFactorUnordered::allocate(const VariableSlots& variableSlots, vector< + // size_t>& varDims, size_t m) { + // keys_.resize(variableSlots.size()); + // std::transform(variableSlots.begin(), variableSlots.end(), begin(), + // boost::bind(&VariableSlots::const_iterator::value_type::first, _1)); + // varDims.push_back(1); + // Ab_.copyStructureFrom(BlockAb(matrix_, varDims.begin(), varDims.end(), m)); + //} + + /* ************************************************************************* */ + void JacobianFactorUnordered::setModel(bool anyConstrained, const Vector& sigmas) { + if((size_t) sigmas.size() != this->rows()) + throw InvalidNoiseModel(this->rows(), sigmas.size()); + if (anyConstrained) + model_ = noiseModel::Constrained::MixedSigmas(sigmas); + else + model_ = noiseModel::Diagonal::Sigmas(sigmas); + } + +} diff --git a/gtsam/linear/JacobianFactorUnordered.h b/gtsam/linear/JacobianFactorUnordered.h new file mode 100644 index 000000000..8fac740cf --- /dev/null +++ b/gtsam/linear/JacobianFactorUnordered.h @@ -0,0 +1,301 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file JacobianFactor.h + * @author Richard Roberts + * @author Christian Potthast + * @author Frank Dellaert + * @date Dec 8, 2010 + */ +#pragma once + +#include +#include +#include +#include + +#include + +namespace gtsam { + + // Forward declarations + //class HessianFactor; + class VariableSlots; + class GaussianFactorGraphUnordered; + class GaussianConditionalUnordered; + class VectorValuesUnordered; + + /** + * A Gaussian factor in the squared-error form. + * + * JacobianFactor implements a + * Gaussian, which has quadratic negative log-likelihood + * \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f] + * where \f$ \Sigma \f$ is a \a diagonal covariance matrix. The + * matrix \f$ A \f$, r.h.s. vector \f$ b \f$, and diagonal noise model + * \f$ \Sigma \f$ are stored in this class. + * + * This factor represents the sum-of-squares error of a \a linear + * measurement function, and is created upon linearization of a NoiseModelFactor, + * which in turn is a sum-of-squares factor with a nonlinear measurement function. + * + * Here is an example of how this factor represents a sum-of-squares error: + * + * Letting \f$ h(x) \f$ be a \a linear measurement prediction function, \f$ z \f$ be + * the actual observed measurement, the residual is + * \f[ f(x) = h(x) - z . \f] + * If we expect noise with diagonal covariance matrix \f$ \Sigma \f$ on this + * measurement, then the negative log-likelihood of the Gaussian induced by this + * measurement model is + * \f[ E(x) = \frac{1}{2} (h(x) - z)^T \Sigma^{-1} (h(x) - z) . \f] + * Because \f$ h(x) \f$ is linear, we can write it as + * \f[ h(x) = Ax + e \f] + * and thus we have + * \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f] + * where \f$ b = z - e \f$. + * + * This factor can involve an arbitrary number of variables, and in the + * above example \f$ x \f$ would almost always be only be a subset of the variables + * in the entire factor graph. There are special constructors for 1-, 2-, and 3- + * way JacobianFactors, and additional constructors for creating n-way JacobianFactors. + * The Jacobian matrix \f$ A \f$ is passed to these constructors in blocks, + * for example, for a 2-way factor, the constructor would accept \f$ A1 \f$ and \f$ A2 \f$, + * as well as the variable indices \f$ j1 \f$ and \f$ j2 \f$ + * and the negative log-likelihood represented by this factor would be + * \f[ E(x) = \frac{1}{2} (A_1 x_{j1} + A_2 x_{j2} - b)^T \Sigma^{-1} (A_1 x_{j1} + A_2 x_{j2} - b) . \f] + */ + class GTSAM_EXPORT JacobianFactorUnordered : public GaussianFactorUnordered + { + public: + typedef JacobianFactorUnordered This; ///< Typedef to this class + typedef GaussianFactorUnordered Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + + protected: + VerticalBlockMatrix Ab_; // the block view of the full matrix + noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix + + public: + typedef VerticalBlockMatrix::Block ABlock; + typedef VerticalBlockMatrix::constBlock constABlock; + typedef VerticalBlockMatrix::Column BVector; + typedef VerticalBlockMatrix::constColumn constBVector; + + /** Convert from other GaussianFactor */ + explicit JacobianFactorUnordered(const GaussianFactorUnordered& gf); + + /** default constructor for I/O */ + JacobianFactorUnordered(); + + /** Construct Null factor */ + explicit JacobianFactorUnordered(const Vector& b_in); + + /** Construct unary factor */ + JacobianFactorUnordered(Index i1, const Matrix& A1, + const Vector& b, const SharedDiagonal& model); + + /** Construct binary factor */ + JacobianFactorUnordered(Index i1, const Matrix& A1, + Index i2, const Matrix& A2, + const Vector& b, const SharedDiagonal& model); + + /** Construct ternary factor */ + JacobianFactorUnordered(Index i1, const Matrix& A1, Index i2, + const Matrix& A2, Index i3, const Matrix& A3, + const Vector& b, const SharedDiagonal& model); + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + JacobianFactorUnordered(const TERMS&terms, const Vector &b, const SharedDiagonal& model); + + /** Convert from a HessianFactor (does Cholesky) */ + //JacobianFactorUnordered(const HessianFactor& factor); + + /** Build a dense joint factor from all the factors in a factor graph. */ + explicit JacobianFactorUnordered(const GaussianFactorGraphUnordered& gfg); + + /** Virtual destructor */ + virtual ~JacobianFactorUnordered() {} + + /** Clone this JacobianFactorUnordered */ + virtual GaussianFactorUnordered::shared_ptr clone() const { + return boost::static_pointer_cast( + boost::make_shared(*this)); + } + + // Implementing Testable interface + virtual void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + virtual bool equals(const GaussianFactorUnordered& lf, double tol = 1e-9) const; + + Vector unweighted_error(const VectorValuesUnordered& c) const; /** (A*x-b) */ + Vector error_vector(const VectorValuesUnordered& c) const; /** (A*x-b)/sigma */ + virtual double error(const VectorValuesUnordered& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */ + + /** Return the augmented information matrix represented by this GaussianFactor. + * The augmented information matrix contains the information matrix with an + * additional column holding the information vector, and an additional row + * holding the transpose of the information vector. The lower-right entry + * contains the constant error term (when \f$ \delta x = 0 \f$). The + * augmented information matrix is described in more detail in HessianFactor, + * which in fact stores an augmented information matrix. + */ + virtual Matrix augmentedInformation() const; + + /** Return the non-augmented information matrix represented by this + * GaussianFactor. + */ + virtual Matrix information() const; + + /** + * Construct the corresponding anti-factor to negate information + * stored stored in this factor. + * @return a HessianFactor with negated Hessian matrices + */ + //virtual GaussianFactorUnordered::shared_ptr negate() const; + + /** Check if the factor contains no information, i.e. zero rows. This does + * not necessarily mean that the factor involves no variables (to check for + * involving no variables use keys().empty()). + */ + bool empty() const { return Ab_.rows() == 0;} + + /** is noise model constrained ? */ + bool isConstrained() const { return model_->isConstrained(); } + + /** Return the dimension of the variable pointed to by the given key iterator + * todo: Remove this in favor of keeping track of dimensions with variables? + */ + virtual size_t getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); } + + /** + * return the number of rows in the corresponding linear system + */ + size_t rows() const { return Ab_.rows(); } + + /** + * return the number of rows in the corresponding linear system + */ + size_t numberOfRows() const { return rows(); } + + /** + * return the number of columns in the corresponding linear system + */ + size_t cols() const { return Ab_.cols(); } + + /** get a copy of model */ + const SharedDiagonal& get_model() const { return model_; } + + /** get a copy of model (non-const version) */ + SharedDiagonal& get_model() { return model_; } + + /** Get a view of the r.h.s. vector b */ + const constBVector getb() const { return Ab_(size()).col(0); } + + /** Get a view of the A matrix for the variable pointed to by the given key iterator */ + constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); } + + /** Get a view of the A matrix */ + constABlock getA() const { return Ab_.range(0, size()); } + + /** Get a view of the r.h.s. vector b (non-const version) */ + BVector getb() { return Ab_(size()).col(0); } + + /** Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version) */ + ABlock getA(iterator variable) { return Ab_(variable - begin()); } + + /** Get a view of the A matrix */ + ABlock getA() { return Ab_.range(0, size()); } + + /** Return A*x */ + Vector operator*(const VectorValuesUnordered& x) const; + + /** x += A'*e */ + void transposeMultiplyAdd(double alpha, const Vector& e, VectorValuesUnordered& x) const; + + /** + * Return (dense) matrix associated with factor + * @param ordering of variables needed for matrix column order + * @param set weight to true to bake in the weights + */ + std::pair matrix(bool weight = true) const; + + /** + * Return (dense) matrix associated with factor + * The returned system is an augmented matrix: [A b] + * @param set weight to use whitening to bake in weights + */ + Matrix matrix_augmented(bool weight = true) const; + + /** + * Return vector of i, j, and s to generate an m-by-n sparse matrix + * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. + * As above, the standard deviations are baked into A and b + * @param columnIndices First column index for each variable. + */ + std::vector > + sparse(const std::vector& columnIndices) const; + + /** + * Return a whitened version of the factor, i.e. with unit diagonal noise + * model. */ + JacobianFactorUnordered whiten() const; + + /** Eliminate the first variable, modifying the factor in place to contain the remaining marginal. */ + boost::shared_ptr eliminateFirst(); + + /** Eliminate the requested number of frontal variables, modifying the factor in place to contain the remaining marginal. */ + boost::shared_ptr eliminate(size_t nrFrontals = 1); + + /** + * splits a pre-factorized factor into a conditional, and changes the current + * factor to be the remaining component. Performs same operation as eliminate(), + * but without running QR. + */ + boost::shared_ptr splitConditional(size_t nrFrontals = 1); + + // following methods all used in CombineJacobians: + // Many imperative, perhaps all need to be combined in constructor + + /** allocate space */ + //void allocate(const VariableSlots& variableSlots, + // std::vector& varDims, size_t m); + + /** set noiseModel correctly */ + void setModel(bool anyConstrained, const Vector& sigmas); + + /** Assert invariants after elimination */ + void assertInvariants() const; + + private: + + /// Internal function to fill blocks and set dimensions + template + void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel); + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(Ab_); + ar & BOOST_SERIALIZATION_NVP(model_); + } + }; // JacobianFactor + +} // gtsam + +#include + + diff --git a/gtsam/linear/VectorValuesUnordered.cpp b/gtsam/linear/VectorValuesUnordered.cpp new file mode 100644 index 000000000..abdf9bf65 --- /dev/null +++ b/gtsam/linear/VectorValuesUnordered.cpp @@ -0,0 +1,236 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file VectorValues.cpp + * @brief Implementations for VectorValues + * @author Richard Roberts + * @author Alex Cunningham + */ + +#include +#include + +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +VectorValuesUnordered VectorValuesUnordered::Zero(const VectorValuesUnordered& x) { + VectorValuesUnordered result; + result.values_.resize(x.size()); + for(size_t j=0; j VectorValuesUnordered::dims() const { + std::vector result(this->size()); + for(Index j = 0; j < this->size(); ++j) + result[j] = this->dim(j); + return result; +} + +/* ************************************************************************* */ +void VectorValuesUnordered::insert(Index j, const Vector& value) { + // Make sure j does not already exist + if(exists(j)) + throw invalid_argument("VectorValues: requested variable index to insert already exists."); + + // If this adds variables at the end, insert zero-length entries up to j + if(j >= size()) + values_.resize(j+1); + + // Assign value + values_[j] = value; +} + +/* ************************************************************************* */ +void VectorValuesUnordered::print(const std::string& str, const IndexFormatter& formatter) const { + std::cout << str << ": " << size() << " elements\n"; + for (Index var = 0; var < size(); ++var) + std::cout << " " << formatter(var) << ": \n" << (*this)[var] << "\n"; + std::cout.flush(); +} + +/* ************************************************************************* */ +bool VectorValuesUnordered::equals(const VectorValuesUnordered& x, double tol) const { + if(this->size() != x.size()) + return false; + for(Index j=0; j < size(); ++j) + if(!equal_with_abs_tol(values_[j], x.values_[j], tol)) + return false; + return true; +} + +/* ************************************************************************* */ +void VectorValuesUnordered::resize(Index nVars, size_t varDim) { + values_.resize(nVars); + for(Index j = 0; j < nVars; ++j) + values_[j] = Vector(varDim); +} + +/* ************************************************************************* */ +void VectorValuesUnordered::resizeLike(const VectorValuesUnordered& other) { + values_.resize(other.size()); + for(Index j = 0; j < other.size(); ++j) + values_[j].resize(other.values_[j].size()); +} + +/* ************************************************************************* */ +VectorValuesUnordered VectorValuesUnordered::SameStructure(const VectorValuesUnordered& other) { + VectorValuesUnordered ret; + ret.resizeLike(other); + return ret; +} + +/* ************************************************************************* */ +VectorValuesUnordered VectorValuesUnordered::Zero(Index nVars, size_t varDim) { + VectorValuesUnordered ret(nVars, varDim); + ret.setZero(); + return ret; +} + +/* ************************************************************************* */ +void VectorValuesUnordered::setZero() { + BOOST_FOREACH(Vector& v, *this) { + v.setZero(); + } +} + +/* ************************************************************************* */ +const Vector VectorValuesUnordered::asVector() const { + return internal::extractVectorValuesSlices(*this, + boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(this->size()), true); +} + +/* ************************************************************************* */ +const Vector VectorValuesUnordered::vector(const std::vector& indices) const { + return internal::extractVectorValuesSlices(*this, indices.begin(), indices.end()); +} + +/* ************************************************************************* */ +bool VectorValuesUnordered::hasSameStructure(const VectorValuesUnordered& other) const { + if(this->size() != other.size()) + return false; + for(size_t j = 0; j < size(); ++j) + // Directly accessing maps instead of using VV::dim in case some values are empty + if(this->values_[j].rows() != other.values_[j].rows()) + return false; + return true; +} + +/* ************************************************************************* */ +void VectorValuesUnordered::permuteInPlace(const Permutation& permutation) { + // Allocate new values + Values newValues(this->size()); + + // Swap values from this VectorValues to the permuted VectorValues + for(size_t i = 0; i < this->size(); ++i) + newValues[i].swap(this->at(permutation[i])); + + // Swap the values into this VectorValues + values_.swap(newValues); +} + +/* ************************************************************************* */ +void VectorValues::permuteInPlace(const Permutation& selector, const Permutation& permutation) { + if(selector.size() != permutation.size()) + throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes."); + // Create new index the size of the permuted entries + Values reorderedEntries(selector.size()); + // Permute the affected entries into the new index + for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot) + reorderedEntries[dstSlot].swap(values_[selector[permutation[dstSlot]]]); + // Put the affected entries back in the new order + for(size_t slot = 0; slot < selector.size(); ++slot) + values_[selector[slot]].swap(reorderedEntries[slot]); +} + +/* ************************************************************************* */ +void VectorValuesUnordered::swap(VectorValuesUnordered& other) { + this->values_.swap(other.values_); +} + +/* ************************************************************************* */ +double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const { + double result = 0.0; + if(this->size() != v.size()) + throw invalid_argument("VectorValues::dot called with different vector sizes"); + for(Index j = 0; j < this->size(); ++j) + // Directly accessing maps instead of using VV::dim in case some values are empty + if(this->values_[j].size() == v.values_[j].size()) + result += this->values_[j].dot(v.values_[j]); + else + throw invalid_argument("VectorValues::dot called with different vector sizes"); + return result; +} + +/* ************************************************************************* */ +double VectorValuesUnordered::norm() const { + return std::sqrt(this->squaredNorm()); +} + +/* ************************************************************************* */ +double VectorValuesUnordered::squaredNorm() const { + double sumSquares = 0.0; + for(Index j = 0; j < this->size(); ++j) + // Directly accessing maps instead of using VV::dim in case some values are empty + sumSquares += this->values_[j].squaredNorm(); + return sumSquares; +} + +/* ************************************************************************* */ +VectorValuesUnordered VectorValuesUnordered::operator+(const VectorValuesUnordered& c) const { + VectorValuesUnordered result = SameStructure(*this); + if(this->size() != c.size()) + throw invalid_argument("VectorValues::operator+ called with different vector sizes"); + for(Index j = 0; j < this->size(); ++j) + // Directly accessing maps instead of using VV::dim in case some values are empty + if(this->values_[j].size() == c.values_[j].size()) + result.values_[j] = this->values_[j] + c.values_[j]; + else + throw invalid_argument("VectorValues::operator- called with different vector sizes"); + return result; +} + +/* ************************************************************************* */ +VectorValuesUnordered VectorValuesUnordered::operator-(const VectorValuesUnordered& c) const { + VectorValuesUnordered result = SameStructure(*this); + if(this->size() != c.size()) + throw invalid_argument("VectorValues::operator- called with different vector sizes"); + for(Index j = 0; j < this->size(); ++j) + // Directly accessing maps instead of using VV::dim in case some values are empty + if(this->values_[j].size() == c.values_[j].size()) + result.values_[j] = this->values_[j] - c.values_[j]; + else + throw invalid_argument("VectorValues::operator- called with different vector sizes"); + return result; +} + +/* ************************************************************************* */ +void VectorValuesUnordered::operator+=(const VectorValuesUnordered& c) { + if(this->size() != c.size()) + throw invalid_argument("VectorValues::operator+= called with different vector sizes"); + for(Index j = 0; j < this->size(); ++j) + // Directly accessing maps instead of using VV::dim in case some values are empty + if(this->values_[j].size() == c.values_[j].size()) + this->values_[j] += c.values_[j]; + else + throw invalid_argument("VectorValues::operator+= called with different vector sizes"); +} + +/* ************************************************************************* */ + +} // \namespace gtsam diff --git a/gtsam/linear/VectorValuesUnordered.h b/gtsam/linear/VectorValuesUnordered.h new file mode 100644 index 000000000..e6617d947 --- /dev/null +++ b/gtsam/linear/VectorValuesUnordered.h @@ -0,0 +1,455 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file VectorValues.h + * @brief Factor Graph Values + * @author Richard Roberts + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + + /** + * This class represents a collection of vector-valued variables associated + * each with a unique integer index. It is typically used to store the variables + * of a GaussianFactorGraph. Optimizing a GaussianFactorGraph or GaussianBayesNet + * returns this class. + * + * For basic usage, such as receiving a linear solution from gtsam solving functions, + * or creating this class in unit tests and examples where speed is not important, + * you can use a simple interface: + * - The default constructor VectorValues() to create this class + * - insert(Index, const Vector&) to add vector variables + * - operator[](Index) for read and write access to stored variables + * - \ref exists (Index) to check if a variable is present + * - Other facilities like iterators, size(), dim(), etc. + * + * Indices can be non-consecutive and inserted out-of-order, but you should not + * use indices that are larger than a reasonable array size because the indices + * correspond to positions in an internal array. + * + * Example: + * \code + VectorValues values; + values.insert(3, Vector_(3, 1.0, 2.0, 3.0)); + values.insert(4, Vector_(2, 4.0, 5.0)); + values.insert(0, Vector_(4, 6.0, 7.0, 8.0, 9.0)); + + // Prints [ 3.0 4.0 ] + gtsam::print(values[1]); + + // Prints [ 8.0 9.0 ] + values[1] = Vector_(2, 8.0, 9.0); + gtsam::print(values[1]); + \endcode + * + *

Advanced Interface and Performance Information

+ * + * Internally, all vector values are stored as part of one large vector. In + * gtsam this vector is always pre-allocated for efficiency, using the + * advanced interface described below. Accessing and modifying already-allocated + * values is \f$ O(1) \f$. Using the insert() function of the standard interface + * is slow because it requires re-allocating the internal vector. + * + * For advanced usage, or where speed is important: + * - Allocate space ahead of time using a pre-allocating constructor + * (\ref AdvancedConstructors "Advanced Constructors"), Zero(), + * SameStructure(), resize(), or append(). Do not use + * insert(Index, const Vector&), which always has to re-allocate the + * internal vector. + * - The vector() function permits access to the underlying Vector, for + * doing mathematical or other operations that require all values. + * - operator[]() returns a SubVector view of the underlying Vector, + * without copying any data. + * + * Access is through the variable index j, and returns a SubVector, + * which is a view on the underlying data structure. + * + * This class is additionally used in gradient descent and dog leg to store the gradient. + * \nosubgrouping + */ + class GTSAM_EXPORT VectorValuesUnordered { + protected: + typedef std::vector Values; ///< Typedef for the collection of Vectors making up a VectorValues + Values values_; ///< Collection of Vectors making up this VectorValues + + public: + typedef Values::iterator iterator; ///< Iterator over vector values + typedef Values::const_iterator const_iterator; ///< Const iterator over vector values + typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values + typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + + /// @name Standard Constructors + /// @{ + + /** + * Default constructor creates an empty VectorValues. + */ + VectorValuesUnordered() {} + + /** Named constructor to create a VectorValues of the same structure of the + * specified one, but filled with zeros. + * @return + */ + static VectorValuesUnordered Zero(const VectorValuesUnordered& model); + + /// @} + /// @name Standard Interface + /// @{ + + /** Number of variables stored, always 1 more than the highest variable index, + * even if some variables with lower indices are not present. */ + Index size() const { return values_.size(); } + + /** Return the dimension of variable \c j. */ + size_t dim(Index j) const { checkExists(j); return (*this)[j].rows(); } + + /** Return the dimension of each vector in this container */ + std::vector dims() const; + + /** Check whether a variable with index \c j exists. */ + bool exists(Index j) const { return j < size() && values_[j].rows() > 0; } + + /** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Index). */ + Vector& at(Index j) { checkExists(j); return values_[j]; } + + /** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Index). */ + const Vector& at(Index j) const { checkExists(j); return values_[j]; } + + /** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to at(Index). */ + Vector& operator[](Index j) { return at(j); } + + /** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to at(Index). */ + const Vector& operator[](Index j) const { return at(j); } + + /** Insert a vector \c value with index \c j. + * Causes reallocation, but can insert values in any order. + * Throws an invalid_argument exception if the index \c j is already used. + * @param value The vector to be inserted. + * @param j The index with which the value will be associated. + */ + void insert(Index j, const Vector& value); + + iterator begin() { return values_.begin(); } ///< Iterator over variables + const_iterator begin() const { return values_.begin(); } ///< Iterator over variables + iterator end() { return values_.end(); } ///< Iterator over variables + const_iterator end() const { return values_.end(); } ///< Iterator over variables + reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables + const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables + reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables + const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables + + /** print required by Testable for unit testing */ + void print(const std::string& str = "VectorValues: ", + const IndexFormatter& formatter = DefaultIndexFormatter) const; + + /** equals required by Testable for unit testing */ + bool equals(const VectorValuesUnordered& x, double tol = 1e-9) const; + + /// @{ + /// \anchor AdvancedConstructors + /// @name Advanced Constructors + /// @} + + /** Construct from a container of variable dimensions (in variable order), without initializing any values. */ + template + explicit VectorValuesUnordered(const CONTAINER& dimensions) { this->append(dimensions); } + + /** Construct to hold nVars vectors of varDim dimension each. */ + VectorValuesUnordered(Index nVars, size_t varDim) { this->resize(nVars, varDim); } + + /** Named constructor to create a VectorValues that matches the structure of + * the specified VectorValues, but do not initialize the new values. */ + static VectorValuesUnordered SameStructure(const VectorValues& other); + + /** Named constructor to create a VectorValues from a container of variable + * dimensions that is filled with zeros. + * @param dimensions A container of the dimension of each variable to create. + */ + template + static VectorValuesUnordered Zero(const CONTAINER& dimensions); + + /** Named constructor to create a VectorValues filled with zeros that has + * \c nVars variables, each of dimension \c varDim + * @param nVars The number of variables to create + * @param varDim The dimension of each variable + * @return The new VectorValues + */ + static VectorValuesUnordered Zero(Index nVars, size_t varDim); + + /// @} + /// @name Advanced Interface + /// @{ + + /** Resize this VectorValues to have identical structure to other, leaving + * this VectorValues with uninitialized values. + * @param other The VectorValues whose structure to copy + */ + void resizeLike(const VectorValuesUnordered& other); + + /** Resize the VectorValues to hold \c nVars variables, each of dimension + * \c varDim. Any individual vectors that do not change size will keep + * their values, but any new or resized vectors will be uninitialized. + * @param nVars The number of variables to create + * @param varDim The dimension of each variable + */ + void resize(Index nVars, size_t varDim); + + /** Resize the VectorValues to contain variables of the dimensions stored + * in \c dimensions. Any individual vectors that do not change size will keep + * their values, but any new or resized vectors will be uninitialized. + * @param dimensions A container of the dimension of each variable to create. + */ + template + void resize(const CONTAINER& dimensions); + + /** Append to the VectorValues to additionally contain variables of the + * dimensions stored in \c dimensions. The new variables are uninitialized, + * but this function is used to pre-allocate space for performance. This + * function preserves the original data, so all previously-existing variables + * are left unchanged. + * @param dimensions A container of the dimension of each variable to create. + */ + template + void append(const CONTAINER& dimensions); + + /** Removes the last subvector from the VectorValues */ + void pop_back() { values_.pop_back(); }; + + /** Set all entries to zero, does not modify the size. */ + void setZero(); + + /** Retrieve the entire solution as a single vector */ + const Vector asVector() const; + + /** Access a vector that is a subset of relevant indices */ + const Vector vector(const std::vector& indices) const; + + /** Check whether this VectorValues has the same structure, meaning has the + * same number of variables and that all variables are of the same dimension, + * as another VectorValues + * @param other The other VectorValues with which to compare structure + * @return \c true if the structure is the same, \c false if not. + */ + bool hasSameStructure(const VectorValuesUnordered& other) const; + + /** + * Swap the data in this VectorValues with another. + */ + void swap(VectorValuesUnordered& other); + + /// @} + /// @name Linear algebra operations + /// @{ + + /** Dot product with another VectorValues, interpreting both as vectors of + * their concatenated values. */ + double dot(const VectorValuesUnordered& v) const; + + /** Vector L2 norm */ + double norm() const; + + /** Squared vector L2 norm */ + double squaredNorm() const; + + /** + * + operator does element-wise addition. Both VectorValues must have the + * same structure (checked when NDEBUG is not defined). + */ + VectorValuesUnordered operator+(const VectorValuesUnordered& c) const; + + /** + * + operator does element-wise subtraction. Both VectorValues must have the + * same structure (checked when NDEBUG is not defined). + */ + VectorValuesUnordered operator-(const VectorValuesUnordered& c) const; + + /** + * += operator does element-wise addition. Both VectorValues must have the + * same structure (checked when NDEBUG is not defined). + */ + void operator+=(const VectorValuesUnordered& c); + + /// @} + + /// @} + /// @name Matlab syntactic sugar for linear algebra operations + /// @{ + + inline VectorValuesUnordered add(const VectorValuesUnordered& c) const { return *this + c; } + inline VectorValuesUnordered scale(const double a, const VectorValuesUnordered& c) const { return a * (*this); } + + /// @} + + private: + // Throw an exception if j does not exist + void checkExists(Index j) const { + if(!exists(j)) { + const std::string msg = + (boost::format("VectorValues: requested variable index j=%1% is not in this VectorValues.") % j).str(); + throw std::out_of_range(msg); + } + } + + public: + + /** + * scale a vector by a scalar + */ + friend VectorValuesUnordered operator*(const double a, const VectorValuesUnordered &v) { + VectorValues result = VectorValues::SameStructure(v); + for(Index j = 0; j < v.size(); ++j) + result.values_[j] = a * v.values_[j]; + return result; + } + + /// TODO: linear algebra interface seems to have been added for SPCG. + friend void scal(double alpha, VectorValuesUnordered& x) { + for(Index j = 0; j < x.size(); ++j) + x.values_[j] *= alpha; + } + /// TODO: linear algebra interface seems to have been added for SPCG. + friend void axpy(double alpha, const VectorValuesUnordered& x, VectorValuesUnordered& y) { + if(x.size() != y.size()) + throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); + for(Index j = 0; j < x.size(); ++j) + if(x.values_[j].size() == y.values_[j].size()) + y.values_[j] += alpha * x.values_[j]; + else + throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); + } + /// TODO: linear algebra interface seems to have been added for SPCG. + friend void sqrt(VectorValuesUnordered &x) { + for(Index j = 0; j < x.size(); ++j) + x.values_[j] = x.values_[j].cwiseSqrt(); + } + + /// TODO: linear algebra interface seems to have been added for SPCG. + friend void ediv(const VectorValuesUnordered& numerator, const VectorValuesUnordered& denominator, VectorValuesUnordered &result) { + if(numerator.size() != denominator.size() || numerator.size() != result.size()) + throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); + for(Index j = 0; j < numerator.size(); ++j) + if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size()) + result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]); + else + throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); + } + + /// TODO: linear algebra interface seems to have been added for SPCG. + friend void edivInPlace(VectorValuesUnordered& x, const VectorValuesUnordered& y) { + if(x.size() != y.size()) + throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); + for(Index j = 0; j < x.size(); ++j) + if(x.values_[j].size() == y.values_[j].size()) + x.values_[j].array() /= y.values_[j].array(); + else + throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(values_); + } + }; // VectorValues definition + + // Implementations of template and inline functions + + /* ************************************************************************* */ + template + void VectorValuesUnordered::resize(const CONTAINER& dimensions) { + values_.clear(); + append(dimensions); + } + + /* ************************************************************************* */ + template + void VectorValuesUnordered::append(const CONTAINER& dimensions) { + size_t i = size(); + values_.resize(size() + dimensions.size()); + BOOST_FOREACH(size_t dim, dimensions) { + values_[i] = Vector(dim); + ++ i; + } + } + + /* ************************************************************************* */ + template + VectorValuesUnordered VectorValuesUnordered::Zero(const CONTAINER& dimensions) { + VectorValuesUnordered ret; + ret.values_.resize(dimensions.size()); + size_t i = 0; + BOOST_FOREACH(size_t dim, dimensions) { + ret.values_[i] = Vector::Zero(dim); + ++ i; + } + return ret; + } + + namespace internal { + /* ************************************************************************* */ + // Helper function, extracts vectors with variable indices + // in the first and last iterators, and concatenates them in that order into the + // output. + template + const VectorValuesUnordered extractVectorValuesSlices(const VectorValuesUnordered& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) { + // Find total dimensionality + size_t dim = 0; + for(ITERATOR j = first; j != last; ++j) + // If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent) + if(!allowNonexistant || values.exists(*j)) + dim += values.dim(*j); + + // Copy vectors + Vector ret(dim); + size_t varStart = 0; + for(ITERATOR j = first; j != last; ++j) { + // If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent) + if(!allowNonexistant || values.exists(*j)) { + ret.segment(varStart, values.dim(*j)) = values[*j]; + varStart += values.dim(*j); + } + } + return ret; + } + + /* ************************************************************************* */ + // Helper function, writes to the variables in values + // with indices iterated over by first and last, interpreting vector as the + // concatenated vectors to write. + template + void writeVectorValuesSlices(const VECTOR& vector, VectorValuesUnordered& values, ITERATOR first, ITERATOR last) { + // Copy vectors + size_t varStart = 0; + for(ITERATOR j = first; j != last; ++j) { + values[*j] = vector.segment(varStart, values[*j].rows()); + varStart += values[*j].rows(); + } + assert(varStart == vector.rows()); + } + } + +} // \namespace gtsam diff --git a/gtsam/linear/linearExceptions.cpp b/gtsam/linear/linearExceptions.cpp new file mode 100644 index 000000000..1a0ba206f --- /dev/null +++ b/gtsam/linear/linearExceptions.cpp @@ -0,0 +1,46 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file linearExceptions.cpp + * @brief Exceptions that may be thrown by linear solver components + * @author Richard Roberts + * @date Aug 17, 2012 + */ +#pragma once + +#include + +namespace gtsam { + + /* ************************************************************************* */ + const char* InvalidNoiseModel::what() const throw() { + if(description_.empty()) + description_ = (boost::format( + "A JacobianFactor was attempted to be constructed or modified to use a\n" + "noise model of incompatible dimension. The JacobianFactor has\n" + "dimensionality (i.e. length of error vector) %d but the provided noise\n" + "model has dimensionality %d.") % factorDims % noiseModelDims).str(); + return description_.c_str(); + } + + /* ************************************************************************* */ + const char* InvalidMatrixBlock::what() const throw() { + if(description_.empty()) + description_ = (boost::format( + "A JacobianFactor was attempted to be constructed with a matrix block of\n" + "inconsistent dimension. The JacobianFactor has %d rows (i.e. length of\n" + "error vector) but the provided matrix block has %d rows.") + % factorRows % blockRows).str(); + return description_.c_str(); + } + + }