Starting work on linear unordered classes
Richard Roberts
parent
7b3ef381c1
commit
33d55449de
|
|
@ -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 <gtsam/base/VerticalBlockMatrix.h>
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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 <gtsam/base/Matrix.h>
|
||||
|
||||
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<Matrix> Block;
|
||||
typedef Eigen::Block<const Matrix> constBlock;
|
||||
|
||||
// columns of blocks
|
||||
typedef Eigen::Block<Matrix>::ColXpr Column;
|
||||
typedef Eigen::Block<const Matrix>::ConstColXpr constColumn;
|
||||
|
||||
protected:
|
||||
Matrix matrix_; ///< The full matrix
|
||||
std::vector<Index> 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<typename CONTAINER>
|
||||
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<typename ITERATOR>
|
||||
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<typename ITERATOR>
|
||||
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<class ARCHIVE>
|
||||
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_);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -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 <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/inference/BayesNet-inl.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
|
||||
#include <stdarg.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// trick from some reading group
|
||||
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
|
||||
#define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL)
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Explicitly instantiate so we don't have to include everywhere
|
||||
template class BayesNet<GaussianConditional>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianBayesNet scalarGaussian(Index key, double mu, double sigma) {
|
||||
GaussianBayesNet bn;
|
||||
GaussianConditional::shared_ptr
|
||||
conditional(new GaussianConditional(key, Vector_(1,mu)/sigma, eye(1)/sigma, ones(1)));
|
||||
bn.push_back(conditional);
|
||||
return bn;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianBayesNet simpleGaussian(Index key, const Vector& mu, double sigma) {
|
||||
GaussianBayesNet bn;
|
||||
size_t n = mu.size();
|
||||
GaussianConditional::shared_ptr
|
||||
conditional(new GaussianConditional(key, mu/sigma, eye(n)/sigma, ones(n)));
|
||||
bn.push_back(conditional);
|
||||
return bn;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R,
|
||||
Index name1, Matrix S, Vector sigmas) {
|
||||
GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, sigmas));
|
||||
bn.push_front(cg);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R,
|
||||
Index name1, Matrix S, Index name2, Matrix T, Vector sigmas) {
|
||||
GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, name2, T, sigmas));
|
||||
bn.push_front(cg);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn) {
|
||||
vector<size_t> dimensions(bn.size());
|
||||
Index var = 0;
|
||||
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> conditional, bn) {
|
||||
dimensions[var++] = conditional->dim();
|
||||
}
|
||||
return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues optimize(const GaussianBayesNet& bn) {
|
||||
VectorValues x = *allocateVectorValues(bn);
|
||||
optimizeInPlace(bn, x);
|
||||
return x;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
|
||||
void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x) {
|
||||
/** solve each node in turn in topological sort order (parents first)*/
|
||||
BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> 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<const GaussianConditional> cg, bn) {
|
||||
const Index key = *(cg->beginFrontals());
|
||||
Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents());
|
||||
xS = input[key] - cg->get_S() * xS;
|
||||
output[key] = cg->get_R().triangularView<Eigen::Upper>().solve(xS);
|
||||
}
|
||||
|
||||
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> 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<const GaussianConditional> 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<JacobianFactor> 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,Vector> matrix(const GaussianBayesNet& bn) {
|
||||
|
||||
// add the dimensions of all variables to get matrix dimension
|
||||
// and at the same time create a mapping from keys to indices
|
||||
size_t N=0; map<Index,size_t> mapping;
|
||||
BOOST_FOREACH(GaussianConditional::shared_ptr cg,bn) {
|
||||
GaussianConditional::const_iterator it = cg->beginFrontals();
|
||||
for (; it != cg->endFrontals(); ++it) {
|
||||
mapping.insert(make_pair(*it,N));
|
||||
N += cg->dim(it);
|
||||
}
|
||||
}
|
||||
|
||||
// create matrix and copy in values
|
||||
Matrix R = zeros(N,N);
|
||||
Vector d(N);
|
||||
Index key; size_t I;
|
||||
FOREACH_PAIR(key,I,mapping) {
|
||||
// find corresponding conditional
|
||||
boost::shared_ptr<const GaussianConditional> cg = bn[key];
|
||||
|
||||
// get sigmas
|
||||
Vector sigmas = cg->get_sigmas();
|
||||
|
||||
// get RHS and copy to d
|
||||
GaussianConditional::const_d_type d_ = cg->get_d();
|
||||
const size_t n = d_.size();
|
||||
for (size_t i=0;i<n;i++)
|
||||
d(I+i) = d_(i)/sigmas(i);
|
||||
|
||||
// get leading R matrix and copy to R
|
||||
GaussianConditional::const_r_type R_ = cg->get_R();
|
||||
for (size_t i=0;i<n;i++)
|
||||
for(size_t j=0;j<n;j++)
|
||||
R(I+i,I+j) = R_(i,j)/sigmas(i);
|
||||
|
||||
// loop over S matrices and copy them into R
|
||||
GaussianConditional::const_iterator keyS = cg->beginParents();
|
||||
for (; keyS!=cg->endParents(); keyS++) {
|
||||
Matrix S = cg->get_S(keyS); // get S matrix
|
||||
const size_t m = S.rows(), n = S.cols(); // find S size
|
||||
const size_t J = mapping[*keyS]; // find column index
|
||||
for (size_t i=0;i<m;i++)
|
||||
for(size_t j=0;j<n;j++)
|
||||
R(I+i,J+j) = S(i,j)/sigmas(i);
|
||||
} // keyS
|
||||
|
||||
} // keyI
|
||||
|
||||
return make_pair(R,d);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double determinant(const GaussianBayesNet& bayesNet) {
|
||||
double logDet = 0.0;
|
||||
|
||||
BOOST_FOREACH(boost::shared_ptr<const GaussianConditional> cg, bayesNet){
|
||||
logDet += cg->get_R().diagonal().unaryExpr(ptr_fun<double,double>(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
|
||||
|
|
@ -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 <gtsam/global_includes.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/inference/BayesNet.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** A Bayes net made from linear-Gaussian densities */
|
||||
typedef BayesNet<GaussianConditional> GaussianBayesNet;
|
||||
|
||||
/** 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<VectorValues> 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, Vector> matrix(const GaussianBayesNet&);
|
||||
|
||||
/**
|
||||
* Computes the determinant of a GassianBayesNet
|
||||
* A GaussianBayesNet is an upper triangular matrix and for an upper triangular matrix
|
||||
* determinant is the product of the diagonal elements. Instead of actually multiplying
|
||||
* we add the logarithms of the diagonal elements and take the exponent at the end
|
||||
* because this is more numerically stable.
|
||||
* @param bayesNet The input GaussianBayesNet
|
||||
* @return The determinant
|
||||
*/
|
||||
GTSAM_EXPORT double determinant(const GaussianBayesNet& bayesNet);
|
||||
|
||||
/**
|
||||
* 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
|
||||
|
|
@ -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 <boost/foreach.hpp>
|
||||
|
||||
#include <gtsam/linear/GaussianBayesTree.h> // Only to help Eclipse
|
||||
|
||||
#include <stdarg.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace internal {
|
||||
template<class BAYESTREE>
|
||||
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<BAYESTREE>(child, result);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE>
|
||||
double logDeterminant(const typename BAYESTREE::sharedClique& clique) {
|
||||
double result = 0.0;
|
||||
|
||||
// this clique
|
||||
result += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
|
||||
|
||||
// sum of children
|
||||
BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_)
|
||||
result += logDeterminant<BAYESTREE>(child);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // \namespace internal
|
||||
} // \namespace gtsam
|
||||
|
|
@ -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 <gtsam/linear/GaussianBayesTree.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
|
||||
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<GaussianBayesTree>(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<JacobianFactor> 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<JacobianFactor>(bayesTree), x0);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g) {
|
||||
gradientAtZero(FactorGraph<JacobianFactor>(bayesTree), g);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double determinant(const GaussianBayesTree& bayesTree) {
|
||||
if (!bayesTree.root())
|
||||
return 0.0;
|
||||
|
||||
return exp(internal::logDeterminant<GaussianBayesTree>(bayesTree.root()));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -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 <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// A Bayes Tree representing a Gaussian density
|
||||
GTSAM_EXPORT typedef BayesTree<GaussianConditional> 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<class BAYESTREE>
|
||||
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<class BAYESTREE>
|
||||
double logDeterminant(const typename BAYESTREE::sharedClique& clique);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#include <gtsam/linear/GaussianBayesTree-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 <boost/range/join.hpp>
|
||||
#include <boost/assign/list_of.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename PARENTS>
|
||||
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<typename TERMS>
|
||||
GaussianConditionalUnordered::GaussianConditionalUnordered(const TERMS& terms,
|
||||
size_t nrFrontals, const Vector& d, const SharedDiagonal& sigmas) :
|
||||
BaseFactor(terms, d, sigmas), BaseConditional(nrFrontals) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename ITERATOR>
|
||||
GaussianConditional::shared_ptr GaussianConditional::Combine(ITERATOR firstConditional, ITERATOR lastConditional) {
|
||||
|
||||
// TODO: check for being a clique
|
||||
|
||||
// Get dimensions from first conditional
|
||||
std::vector<size_t> 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<Matrix> 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
|
||||
|
|
@ -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 <string.h>
|
||||
#include <boost/format.hpp>
|
||||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wunused-variable"
|
||||
#endif
|
||||
#include <boost/lambda/lambda.hpp>
|
||||
#include <boost/lambda/bind.hpp>
|
||||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic pop
|
||||
#endif
|
||||
|
||||
#include <gtsam/linear/linearExceptions.h>
|
||||
#include <gtsam/linear/GaussianConditionalUnordered.h>
|
||||
#include <gtsam/linear/VectorValuesUnordered.h>
|
||||
|
||||
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<Ab_.rows(); i++) {
|
||||
list<Vector> rows1; rows1.push_back(Vector(get_R().row(i)));
|
||||
list<Vector> 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<Eigen::Upper>().solve(xS);
|
||||
|
||||
// Check for indeterminant solution
|
||||
if(soln.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite<double>), boost::lambda::_1)).any())
|
||||
throw IndeterminantLinearSystemException(this->keys().front());
|
||||
|
||||
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<double>), 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());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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 <boost/utility.hpp>
|
||||
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/linear/JacobianFactorUnordered.h>
|
||||
#include <gtsam/inference/ConditionalUnordered.h>
|
||||
#include <gtsam/linear/VectorValuesUnordered.h>
|
||||
|
||||
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<JacobianFactorUnordered, GaussianConditionalUnordered>
|
||||
{
|
||||
public:
|
||||
typedef GaussianConditionalUnordered This; ///< Typedef to this class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
|
||||
typedef JacobianFactorUnordered BaseFactor; ///< Typedef to our factor base class
|
||||
typedef ConditionalUnordered<BaseFactor, This> 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<Key, Matrix>, specifying the
|
||||
* collection of parent keys and matrices. */
|
||||
template<typename PARENTS>
|
||||
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<Key, Matrix>, specifying the
|
||||
* collection of keys and matrices making up the conditional. */
|
||||
template<typename TERMS>
|
||||
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<GaussianConditional>.
|
||||
* @param lastConditional Iterator to after the last conditional to combine, must dereference
|
||||
* to a shared_ptr<GaussianConditional>. */
|
||||
template<typename ITERATOR>
|
||||
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<class Archive>
|
||||
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 <gtsam/linear/GaussianConditionalUnordered-inl.h>
|
||||
|
||||
|
|
@ -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 <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/cholesky.h>
|
||||
#include <gtsam/base/FastVector.h>
|
||||
#include <gtsam/linear/HessianFactor.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/inference/VariableSlots.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/cholesky.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : Base(CBN) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
|
||||
FastSet<Index> 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<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
|
||||
// First find dimensions of each variable
|
||||
FastVector<size_t> 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<size_t> columnIndices(dims.size()+1, 0);
|
||||
for(size_t j=1; j<dims.size()+1; ++j)
|
||||
columnIndices[j] = columnIndices[j-1] + dims[j-1];
|
||||
|
||||
// Iterate over all factors, adding sparse scalar entries
|
||||
typedef boost::tuple<size_t, size_t, double> triplet;
|
||||
FastVector<triplet> entries;
|
||||
size_t row = 0;
|
||||
BOOST_FOREACH(const sharedFactor& factor, *this) {
|
||||
// Convert to JacobianFactor if necessary
|
||||
JacobianFactor::shared_ptr jacobianFactor(
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(factor));
|
||||
if (!jacobianFactor) {
|
||||
HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(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<whitened.end(); ++pos) {
|
||||
JacobianFactor::constABlock whitenedA = whitened.getA(pos);
|
||||
// find first column index for this key
|
||||
size_t column_start = columnIndices[*pos];
|
||||
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(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<triplet>(entries.begin(), entries.end());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix GaussianFactorGraph::sparseJacobian_() const {
|
||||
|
||||
// call sparseJacobian
|
||||
typedef boost::tuple<size_t, size_t, double> triplet;
|
||||
std::vector<triplet> 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<JacobianFactor> jfg;
|
||||
jfg.reserve(this->size());
|
||||
BOOST_FOREACH(const sharedFactor& factor, *this) {
|
||||
if(boost::shared_ptr<JacobianFactor> jf =
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(factor))
|
||||
jfg.push_back(jf);
|
||||
else
|
||||
jfg.push_back(boost::make_shared<JacobianFactor>(*factor));
|
||||
}
|
||||
// combine all factors
|
||||
JacobianFactor combined(*CombineJacobians(jfg, VariableSlots(*this)));
|
||||
return combined.matrix_augmented();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<Matrix,Vector> 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<vector<size_t>, size_t, size_t> countDims(const FactorGraph<JacobianFactor>& factors, const VariableSlots& variableSlots) {
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
|
||||
#else
|
||||
vector<size_t> 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<Index>::max()) {
|
||||
const JacobianFactor& sourceFactor = *factors[sourceFactorI];
|
||||
size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
if(varDims[jointVarpos] == numeric_limits<size_t>::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<JacobianFactor>& 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<size_t> 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<Index>::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<Eigen::StrictlyLower>() = result.transpose();
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<Matrix,Vector> 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<JacobianFactor> convertToJacobians(const FactorGraph<
|
||||
GaussianFactor>& factors) {
|
||||
|
||||
typedef JacobianFactor J;
|
||||
typedef HessianFactor H;
|
||||
|
||||
const bool debug = ISDEBUG("convertToJacobians");
|
||||
|
||||
FactorGraph<J> jacobians;
|
||||
jacobians.reserve(factors.size());
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
|
||||
if (factor) {
|
||||
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
|
||||
if (jacobian) {
|
||||
jacobians.push_back(jacobian);
|
||||
if (debug) jacobian->print("Existing JacobianFactor: ");
|
||||
} else {
|
||||
H::shared_ptr hessian(boost::dynamic_pointer_cast<H>(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<JacobianFactor> 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<GaussianFactor>& factors) {
|
||||
typedef JacobianFactor J;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
|
||||
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
|
||||
if (jacobian && jacobian->get_model()->isConstrained()) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::EliminationResult EliminatePreferCholesky(
|
||||
const FactorGraph<GaussianFactor>& 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<JacobianFactor>(gf);
|
||||
if( !result ) {
|
||||
result = boost::make_shared<JacobianFactor>(*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<Errors> gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x) {
|
||||
boost::shared_ptr<Errors> 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
|
||||
|
|
@ -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 <gtsam/inference/FactorGraphUnordered.h>
|
||||
#include <gtsam/inference/EliminateableFactorGraph.h>
|
||||
#include <gtsam/linear/GaussianFactorUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declaration to use as default argument, documented declaration below.
|
||||
GTSAM_EXPORT FactorGraph<GaussianFactor>::EliminationResult
|
||||
EliminateQR(const FactorGraph<GaussianFactor>& 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<GaussianFactor> {
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<GaussianFactorGraph> shared_ptr;
|
||||
typedef FactorGraph<GaussianFactor> 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<class CLIQUE>
|
||||
GaussianFactorGraph(const BayesTree<GaussianConditional,CLIQUE>& gbt) : Base(gbt) {}
|
||||
|
||||
/** Constructor from a factor graph of GaussianFactor or a derived type */
|
||||
template<class DERIVEDFACTOR>
|
||||
GaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& 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<std::pair<Index, Matrix> > &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<Index> 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<GaussianFactor>::eliminateFrontals with EliminateQR as the
|
||||
* eliminate function argument.
|
||||
*/
|
||||
std::pair<sharedConditional, GaussianFactorGraph> 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<GaussianFactor>::eliminate with EliminateQR as the eliminate
|
||||
* function argument.
|
||||
*/
|
||||
std::pair<sharedConditional, GaussianFactorGraph> eliminate(const std::vector<Index>& variables, const Eliminate& function = EliminateQR) const {
|
||||
return Base::eliminate(variables, function); }
|
||||
|
||||
/** Eliminate a single variable, by calling GaussianFactorGraph::eliminate. */
|
||||
std::pair<sharedConditional, GaussianFactorGraph> 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<boost::tuple<size_t, size_t, double> > 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<Matrix,Vector> 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<Matrix,Vector> hessian() const;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
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<JacobianFactor>& 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<GaussianFactor>& 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<Errors> 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
|
||||
|
|
@ -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 <gtsam/base/Matrix.h>
|
||||
#include <gtsam/inference/FactorUnordered.h>
|
||||
|
||||
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<This> 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<typename CONTAINER>
|
||||
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<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
}
|
||||
|
||||
}; // GaussianFactorUnordered
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -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 <sstream>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/format.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wunused-variable"
|
||||
#endif
|
||||
#include <boost/bind.hpp>
|
||||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic pop
|
||||
#endif
|
||||
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
#include <gtsam/base/cholesky.h>
|
||||
#include <gtsam/linear/linearExceptions.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/linear/HessianFactor.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
string SlotEntry::toString() const {
|
||||
ostringstream oss;
|
||||
oss << "SlotEntry: slot=" << slot << ", dim=" << dimension;
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Scatter::Scatter(const FactorGraph<GaussianFactor>& 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<size_t> 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<Index>& js, const std::vector<Matrix>& Gs,
|
||||
const std::vector<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<const JacobianFactor*>(&gf)) {
|
||||
const JacobianFactor& jf(static_cast<const JacobianFactor&>(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<const HessianFactor*>(&gf)) {
|
||||
const HessianFactor& hf(static_cast<const HessianFactor&>(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<GaussianFactor>& factors) : info_(matrix_)
|
||||
{
|
||||
Scatter scatter(factors);
|
||||
|
||||
// Pull out keys and dimensions
|
||||
gttic(keys);
|
||||
vector<size_t> 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<HessianFactor>(factor))
|
||||
updateATA(*hessian, scatter);
|
||||
else if(JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactor>(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<Eigen::Upper>()), "Ab^T * Ab: ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
|
||||
if(!dynamic_cast<const HessianFactor*>(&lf))
|
||||
return false;
|
||||
else {
|
||||
Matrix thisMatrix = this->info_.full().selfadjointView<Eigen::Upper>();
|
||||
thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
|
||||
Matrix rhsMatrix = static_cast<const HessianFactor&>(lf).info_.full().selfadjointView<Eigen::Upper>();
|
||||
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<Eigen::Upper>();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix HessianFactor::information() const {
|
||||
return info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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<Eigen::Upper>() * 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; j2<update.info_.nBlocks(); ++j2) {
|
||||
size_t slot2 = (j2 == update.size()) ? this->info_.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<Eigen::Upper>() +=
|
||||
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<const JacobianFactor::AbMatrix> 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<noiseModel::Unit>(update.model_)) {
|
||||
updateInform.noalias() = updateA.transpose() * updateA;
|
||||
} else {
|
||||
noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(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; j2<update.Ab_.nBlocks(); ++j2) {
|
||||
size_t slot2 = (j2 == update.size()) ? this->info_.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<Eigen::Upper>() +=
|
||||
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<Matrix> 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<ConditionalType>(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<Index> 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<Index> js;
|
||||
std::vector<Matrix> Gs;
|
||||
std::vector<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
|
||||
|
|
@ -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 <gtsam/base/blockMatrices.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
|
||||
// Forward declarations for friend unit tests
|
||||
class HessianFactorConversionConstructorTest;
|
||||
class HessianFactorConstructor1Test;
|
||||
class HessianFactorConstructor1bTest;
|
||||
class HessianFactorcombineTest;
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
class JacobianFactor;
|
||||
class GaussianConditional;
|
||||
template<class C> 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<Index, SlotEntry> {
|
||||
public:
|
||||
Scatter() {}
|
||||
Scatter(const FactorGraph<GaussianFactor>& 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<InfoMatrix> 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<HessianFactor> 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<Index>& js, const std::vector<Matrix>& Gs,
|
||||
const std::vector<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<GaussianFactor>& 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<GaussianFactor>(
|
||||
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 <em>upper-triangular part</em> 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 <em>upper-triangular part</em> 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 <em>upper-triangular part</em> 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 <em>upper-triangular part</em> 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<GaussianConditional> 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<class ARCHIVE>
|
||||
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_);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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 <boost/range/adaptor/map.hpp>
|
||||
#include <boost/range/adaptor/transformed.hpp>
|
||||
#include <boost/range/join.hpp>
|
||||
#include <boost/assign/list_of.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename TERMS>
|
||||
JacobianFactorUnordered::JacobianFactorUnordered(const TERMS&terms, const Vector &b, const SharedDiagonal& model)
|
||||
{
|
||||
fillTerms(terms, b, model);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename TERMS>
|
||||
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<Key, Matrix> 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
|
||||
|
||||
|
|
@ -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 <gtsam/linear/linearExceptions.h>
|
||||
#include <gtsam/linear/GaussianConditionalUnordered.h>
|
||||
#include <gtsam/linear/JacobianFactorUnordered.h>
|
||||
#include <gtsam/linear/HessianFactorUnordered.h>
|
||||
#include <gtsam/linear/GaussianFactorGraphUnordered.h>
|
||||
#include <gtsam/linear/VectorValuesUnordered.h>
|
||||
#include <gtsam/inference/VariableSlots.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
#include <gtsam/base/cholesky.h>
|
||||
|
||||
#include <boost/assign/list_of.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/format.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <boost/array.hpp>
|
||||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wunused-variable"
|
||||
#endif
|
||||
#include <boost/bind.hpp>
|
||||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic pop
|
||||
#endif
|
||||
|
||||
#include <cmath>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
|
||||
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<const JacobianFactorUnordered*>(&gf))
|
||||
*this = JacobianFactorUnordered(*rhs);
|
||||
//else if(const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&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<Eigen::StrictlyLower>() =
|
||||
// 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<JacobianFactorUnordered::shared_ptr> jacobians;
|
||||
jacobians.reserve(gfg.size());
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& factor, gfg) {
|
||||
if(factor) {
|
||||
if(JacobianFactorUnordered::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactorUnordered>(factor))
|
||||
jacobians.push_back(jf);
|
||||
else
|
||||
jacobians.push_back(boost::make_shared<JacobianFactorUnordered>(*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<const JacobianFactorUnordered*>(&f_))
|
||||
return false;
|
||||
else {
|
||||
const JacobianFactorUnordered& f(static_cast<const JacobianFactorUnordered&>(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; pos<size(); ++pos)
|
||||
e += Ab_(pos) * c[keys_[pos]];
|
||||
return e;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector JacobianFactorUnordered::error_vector(const VectorValuesUnordered& c) const {
|
||||
if (empty()) return model_->whiten(-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; pos<size(); ++pos)
|
||||
Ax += Ab_(pos) * x[keys_[pos]];
|
||||
|
||||
return model_->whiten(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<size(); ++pos)
|
||||
gtsam::transposeMultiplyAdd(1.0, Ab_(pos), E, x[keys_[pos]]);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix,Vector> 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<boost::tuple<size_t, size_t, double> >
|
||||
JacobianFactorUnordered::sparse(const std::vector<size_t>& columnIndices) const {
|
||||
|
||||
std::vector<boost::tuple<size_t, size_t, double> > entries;
|
||||
|
||||
// iterate over all variables in the factor
|
||||
for(const_iterator var=begin(); var<end(); ++var) {
|
||||
Matrix whitenedA(model_->Whiten(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<const Vector> 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; i<noiseModel->dim(); ++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);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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 <gtsam/linear/GaussianFactorUnordered.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/base/VerticalBlockMatrix.h>
|
||||
#include <gtsam/global_includes.h>
|
||||
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
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<This> 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<Key, Matrix>, specifying the
|
||||
* collection of keys and matrices making up the factor. */
|
||||
template<typename TERMS>
|
||||
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<GaussianFactorUnordered>(
|
||||
boost::make_shared<JacobianFactorUnordered>(*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, Vector> 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<boost::tuple<size_t, size_t, double> >
|
||||
sparse(const std::vector<size_t>& 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<GaussianConditionalUnordered> eliminateFirst();
|
||||
|
||||
/** Eliminate the requested number of frontal variables, modifying the factor in place to contain the remaining marginal. */
|
||||
boost::shared_ptr<GaussianConditionalUnordered> 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<GaussianConditionalUnordered> 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<size_t>& 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<typename TERMS>
|
||||
void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
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 <gtsam/linear/JacobianFactorUnordered-inl.h>
|
||||
|
||||
|
||||
|
|
@ -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 <gtsam/base/FastVector.h>
|
||||
#include <gtsam/linear/VectorValuesUnordered.h>
|
||||
|
||||
#include <boost/iterator/counting_iterator.hpp>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValuesUnordered VectorValuesUnordered::Zero(const VectorValuesUnordered& x) {
|
||||
VectorValuesUnordered result;
|
||||
result.values_.resize(x.size());
|
||||
for(size_t j=0; j<x.size(); ++j)
|
||||
result.values_[j] = Vector::Zero(x.values_[j].size());
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
vector<size_t> VectorValuesUnordered::dims() const {
|
||||
std::vector<size_t> 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<Index>& 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
|
||||
|
|
@ -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 <gtsam/base/Vector.h>
|
||||
#include <gtsam/global_includes.h>
|
||||
|
||||
#include <boost/format.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <numeric>
|
||||
#include <stdexcept>
|
||||
|
||||
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
|
||||
*
|
||||
* <h2>Advanced Interface and Performance Information</h2>
|
||||
*
|
||||
* 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<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<VectorValuesUnordered> 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<size_t> 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<class CONTAINER>
|
||||
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<class CONTAINER>
|
||||
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<class CONTAINER>
|
||||
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<class CONTAINER>
|
||||
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<Index>& 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<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(values_);
|
||||
}
|
||||
}; // VectorValues definition
|
||||
|
||||
// Implementations of template and inline functions
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONTAINER>
|
||||
void VectorValuesUnordered::resize(const CONTAINER& dimensions) {
|
||||
values_.clear();
|
||||
append(dimensions);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONTAINER>
|
||||
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<class CONTAINER>
|
||||
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<typename ITERATOR>
|
||||
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<class VECTOR, typename ITERATOR>
|
||||
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
|
||||
|
|
@ -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 <gtsam/linear/linearExceptions.h>
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
}
|
||||
Loading…
Reference in New Issue