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

New SymmetricBlockMatrix interface - should be safer as all exposed functions use symmetric matrix expressions and the internal uninitialized values below the diagonal are never exposed.

release/4.3a0
Richard Roberts 2013-11-26 23:49:53 +00:00
parent 21297238ba
commit 5f87cbea7f
13 changed files with 771 additions and 156 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/base/SymmetricBlockMatrix.cpp
View File

@ -18,6 +18,8 @@
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/base/timing.h>
namespace gtsam {
@ -47,4 +49,30 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial(DenseIndex nFrontals)
{
// Do dense elimination
if(!blockStart() == 0)
throw std::invalid_argument("Can only do Cholesky when the SymmetricBlockMatrix is not a restricted view, i.e. when blockStart == 0.");
if(!gtsam::choleskyPartial(matrix_, offset(nFrontals)))
throw CholeskyFailed();
// Split conditional
// Create one big conditionals with many frontal variables.
gttic(Construct_eliminated);
const size_t varDim = offset(nFrontals);
VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim);
Ab.full() = matrix_.topRows(varDim);
Ab.full().triangularView<Eigen::StrictlyLower>().setZero();
gttoc(Construct_conditional);
gttic(Remaining_factor);
// Take lower-right block of Ab_ to get the remaining factor
blockStart() = nFrontals;
gttoc(Remaining_factor);
return Ab;
}
}

218
gtsam/base/SymmetricBlockMatrix.h
View File

@ -19,6 +19,7 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/SymmetricBlockMatrixBlockExpr.h>
namespace gtsam {
@ -36,12 +37,12 @@ namespace gtsam {
* (except re-setting firstBlock()).
*
* @addtogroup base */
class SymmetricBlockMatrix
class GTSAM_EXPORT SymmetricBlockMatrix
{
public:
typedef SymmetricBlockMatrix This;
typedef Eigen::Block<Matrix> Block;
typedef Eigen::Block<const Matrix> constBlock;
typedef SymmetricBlockMatrixBlockExpr<This> Block;
typedef SymmetricBlockMatrixBlockExpr<const This> constBlock;
protected:
Matrix matrix_; ///< The full matrix
@ -50,7 +51,7 @@ namespace gtsam {
DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment.
public:
/** Construct from an empty matrix (asserts that the matrix is empty) */
/// Construct from an empty matrix (asserts that the matrix is empty)
SymmetricBlockMatrix() :
blockStart_(0)
{
@ -58,7 +59,7 @@ namespace gtsam {
assertInvariants();
}
/** Construct from a container of the sizes of each block. */
/// Construct from a container of the sizes of each block.
template<typename CONTAINER>
SymmetricBlockMatrix(const CONTAINER& dimensions) :
blockStart_(0)
@ -68,8 +69,7 @@ namespace gtsam {
assertInvariants();
}
/**
* Construct from iterator over the sizes of each vertical block. */
/// Construct from iterator over the sizes of each vertical block.
template<typename ITERATOR>
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim) :
blockStart_(0)
@ -79,11 +79,13 @@ namespace gtsam {
assertInvariants();
}
/** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */
/// Construct from a container of the sizes of each vertical block and a pre-prepared matrix.
template<typename CONTAINER>
SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) :
matrix_(matrix), blockStart_(0)
blockStart_(0)
{
matrix_.resize(matrix.rows(), matrix.cols());
matrix_.triangularView<Eigen::Upper>() = matrix.triangularView<Eigen::Upper>();
fillOffsets(dimensions.begin(), dimensions.end());
if(matrix_.rows() != matrix_.cols())
throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix.");
@ -92,118 +94,195 @@ namespace gtsam {
assertInvariants();
}
/** Copy the block structure, but do not copy the matrix data. If blockStart() has been
* modified, this copies the structure of the corresponding matrix view. In the destination
* SymmetricBlockMatrix, blockStart() will be 0. */
/// Copy the block structure, but do not copy the matrix data. If blockStart() has been
/// modified, this copies the structure of the corresponding matrix view. In the destination
/// SymmetricBlockMatrix, blockStart() will be 0.
static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& other);
/** Copy the block structure, but do not copy the matrix data. If blockStart() has been
* modified, this copies the structure of the corresponding matrix view. In the destination
* SymmetricBlockMatrix, blockStart() will be 0. */
/// Copy the block structure, but do not copy the matrix data. If blockStart() has been
/// modified, this copies the structure of the corresponding matrix view. In the destination
/// SymmetricBlockMatrix, blockStart() will be 0.
static SymmetricBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& other);
/** Row size */
/// Row size
DenseIndex rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
/** Column size */
/// Column size
DenseIndex cols() const { return rows(); }
/** Block count */
/// Block count
DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
/// Access the block with vertical block index \c i_block and horizontal block index \c j_block.
/// Note that the actual block accessed in the underlying matrix is relative to blockStart().
Block operator()(DenseIndex i_block, DenseIndex j_block) {
return range(i_block, i_block+1, j_block, j_block+1);
return Block(*this, i_block, j_block);
}
/// Access the block with vertical block index \c i_block and horizontal block index \c j_block.
/// Note that the actual block accessed in the underlying matrix is relative to blockStart().
constBlock operator()(DenseIndex i_block, DenseIndex j_block) const {
return range(i_block, i_block+1, j_block, j_block+1);
return constBlock(*this, i_block, j_block);
}
/// Access the range of blocks starting with vertical block index \c i_startBlock, ending with
/// vertical block index \c i_endBlock, starting with horizontal block index \c j_startBlock,
/// and ending with horizontal block index \c j_endBlock. End block indices are exclusive. Note
/// that the actual blocks accessed in the underlying matrix are relative to blockStart().
Block range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) {
assertInvariants();
DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
DenseIndex j_actualEndBlock = j_endBlock + blockStart_;
checkBlock(i_actualStartBlock);
checkBlock(j_actualStartBlock);
if(i_startBlock != 0 || i_endBlock != 0) {
checkBlock(i_actualStartBlock);
assert(i_actualEndBlock < (DenseIndex)variableColOffsets_.size());
}
if(j_startBlock != 0 || j_endBlock != 0) {
checkBlock(j_actualStartBlock);
assert(j_actualEndBlock < (DenseIndex)variableColOffsets_.size());
}
return matrix_.block(
variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock],
variableColOffsets_[i_actualEndBlock] - variableColOffsets_[i_actualStartBlock],
variableColOffsets_[j_actualEndBlock] - variableColOffsets_[j_actualStartBlock]);
return Block(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock, j_endBlock - j_startBlock);
}
/// Access the range of blocks starting with vertical block index \c i_startBlock, ending with
/// vertical block index \c i_endBlock, starting with horizontal block index \c j_startBlock,
/// and ending with horizontal block index \c j_endBlock. End block indices are exclusive. Note
/// that the actual blocks accessed in the underlying matrix are relative to blockStart().
constBlock range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const {
assertInvariants();
DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
DenseIndex j_actualEndBlock = j_endBlock + blockStart_;
if(i_startBlock != 0 || i_endBlock != 0) {
checkBlock(i_actualStartBlock);
assert(i_actualEndBlock < (DenseIndex)variableColOffsets_.size());
}
if(j_startBlock != 0 || j_endBlock != 0) {
checkBlock(j_actualStartBlock);
assert(j_actualEndBlock < (DenseIndex)variableColOffsets_.size());
}
return matrix_.block(
variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock],
variableColOffsets_[i_actualEndBlock] - variableColOffsets_[i_actualStartBlock],
variableColOffsets_[j_actualEndBlock] - variableColOffsets_[j_actualStartBlock]);
return constBlock(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock, j_endBlock - j_startBlock);
}
/** Return the full matrix, *not* including any portions excluded by firstBlock(). */
Block full() {
return range(0,nBlocks(), 0,nBlocks());
Block full()
{
return Block(*this, 0, nBlocks(), 0);
}
/** Return the full matrix, *not* including any portions excluded by firstBlock(). */
constBlock full() const {
return range(0,nBlocks(), 0,nBlocks());
constBlock full() const
{
return constBlock(*this, 0, nBlocks(), 0);
}
/** Access to full matrix, including any portions excluded by firstBlock() to other operations. */
const Matrix& matrix() const { return matrix_; }
Eigen::SelfAdjointView<const Matrix, Eigen::Upper> matrix() const
{
return matrix_;
}
/** Access to full matrix, including any portions excluded by firstBlock() to other operations. */
Matrix& matrix() { return matrix_; }
Eigen::SelfAdjointView<Matrix, Eigen::Upper> matrix()
{
return matrix_;
}
DenseIndex offset(DenseIndex block) const {
/// Return the absolute offset in the underlying matrix of the start of the specified \c block.
DenseIndex offset(DenseIndex block) const
{
assertInvariants();
DenseIndex actualBlock = block + blockStart_;
checkBlock(actualBlock);
return variableColOffsets_[actualBlock];
}
/// Retrieve or modify the first logical block, i.e. the block referenced by block index 0.
/// Blocks before it will be inaccessible, except by accessing the underlying matrix using
/// matrix().
DenseIndex& blockStart() { return blockStart_; }
/// Retrieve the first logical block, i.e. the block referenced by block index 0. Blocks before
/// it will be inaccessible, except by accessing the underlying matrix using matrix().
DenseIndex blockStart() const { return blockStart_; }
/// Do partial Cholesky in-place and return the eliminated block matrix, leaving the remaining
/// symmetric matrix in place.
VerticalBlockMatrix choleskyPartial(DenseIndex nFrontals);
protected:
void assertInvariants() const {
void assertInvariants() const
{
assert(matrix_.rows() == matrix_.cols());
assert(matrix_.cols() == variableColOffsets_.back());
assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
}
void checkBlock(DenseIndex block) const {
void checkBlock(DenseIndex block) const
{
assert(matrix_.rows() == matrix_.cols());
assert(matrix_.cols() == variableColOffsets_.back());
assert(block >= 0);
assert(block < (DenseIndex)variableColOffsets_.size()-1);
assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
}
DenseIndex offsetUnchecked(DenseIndex block) const
{
return variableColOffsets_[block + blockStart_];
}
//void checkRange(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
//{
// const DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
// const DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
// const DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
// const DenseIndex j_actualEndBlock = j_endBlock + blockStart_;
// checkBlock(i_actualStartBlock);
// checkBlock(j_actualStartBlock);
// if(i_startBlock != 0 || i_endBlock != 0) {
// checkBlock(i_actualStartBlock);
// assert(i_actualEndBlock < (DenseIndex)variableColOffsets_.size());
// }
// if(j_startBlock != 0 || j_endBlock != 0) {
// checkBlock(j_actualStartBlock);
// assert(j_actualEndBlock < (DenseIndex)variableColOffsets_.size());
// }
//}
//void checkRange(DenseIndex startBlock, DenseIndex endBlock) const
//{
// const DenseIndex actualStartBlock = startBlock + blockStart_;
// const DenseIndex actualEndBlock = endBlock + blockStart_;
// checkBlock(actualStartBlock);
// if(startBlock != 0 || endBlock != 0) {
// checkBlock(actualStartBlock);
// assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
// }
//}
//Block rangeUnchecked(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock)
//{
// const DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
// const DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
// const DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
// const DenseIndex j_actualEndBlock = j_endBlock + blockStart_;
// return Block(matrix(),
// variableColOffsets_[i_actualStartBlock],
// variableColOffsets_[j_actualStartBlock],
// variableColOffsets_[i_actualEndBlock] - variableColOffsets_[i_actualStartBlock],
// variableColOffsets_[j_actualEndBlock] - variableColOffsets_[j_actualStartBlock]);
//}
//constBlock rangeUnchecked(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
//{
// // Convert Block to constBlock
// const Block block = const_cast<This*>(this)->rangeUnchecked(i_startBlock, i_endBlock, j_startBlock, j_endBlock);
// return constBlock(matrix(), block.Base::Base::, block.startCol(), block.rows(), block.cols());
//}
//Block rangeUnchecked(DenseIndex startBlock, DenseIndex endBlock)
//{
// const DenseIndex actualStartBlock = startBlock + blockStart_;
// const DenseIndex actualEndBlock = endBlock + blockStart_;
// return Block(matrix(),
// variableColOffsets_[actualStartBlock],
// variableColOffsets_[actualStartBlock],
// variableColOffsets_[actualEndBlock] - variableColOffsets_[actualStartBlock],
// variableColOffsets_[actualEndBlock] - variableColOffsets_[actualStartBlock]);
//}
//constBlock rangeUnchecked(DenseIndex startBlock, DenseIndex endBlock) const
//{
// // Convert Block to constBlock
// const Block block = const_cast<This*>(this)->rangeUnchecked(startBlock, endBlock);
// return constBlock(matrix(), block.startRow(), block.startCol(), block.rows(), block.cols());
//}
template<typename ITERATOR>
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) {
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim)
{
variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
variableColOffsets_[0] = 0;
DenseIndex j=0;
@ -214,6 +293,7 @@ namespace gtsam {
}
friend class VerticalBlockMatrix;
template<typename SymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr;
private:
/** Serialization function */
@ -226,5 +306,13 @@ namespace gtsam {
}
};
/* ************************************************************************* */
class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
{
public:
CholeskyFailed() throw() {}
virtual ~CholeskyFailed() throw() {}
};
}

337
gtsam/base/SymmetricBlockMatrixBlockExpr.h Normal file
View File

@ -0,0 +1,337 @@
/* ----------------------------------------------------------------------------
* 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 SymmetricBlockMatrixBlockExpr.h
* @brief Matrix expression for a block of a SymmetricBlockMatrix
* @author Richard Roberts
* @date Nov 20, 2013
*/
#pragma once
#include <gtsam/base/Matrix.h>
namespace gtsam { template<typename SymmetricBlockMatrixType> class SymmetricBlockMatrixBlockExpr; }
namespace gtsam { class SymmetricBlockMatrix; }
// traits class for Eigen expressions
namespace Eigen
{
namespace internal
{
template<typename SymmetricBlockMatrixType>
struct traits<gtsam::SymmetricBlockMatrixBlockExpr<SymmetricBlockMatrixType> > :
public traits<typename gtsam::const_selector<
SymmetricBlockMatrixType, gtsam::SymmetricBlockMatrix, gtsam::Matrix, const gtsam::Matrix>::type>
{
};
}
}
namespace gtsam
{
/// A matrix expression that references a single block of a SymmetricBlockMatrix. Depending on
/// the position of the block, this expression will behave either as a regular matrix block, a
/// transposed matrix block, or a symmetric matrix block. The only reason this class is templated
/// on SymmetricBlockMatrixType is to allow for both const and non-const references.
template<typename SymmetricBlockMatrixType>
class SymmetricBlockMatrixBlockExpr : public Eigen::EigenBase<SymmetricBlockMatrixBlockExpr<SymmetricBlockMatrixType> >
{
protected:
SymmetricBlockMatrixType& xpr_; ///< The referenced SymmetricBlockMatrix
DenseIndex densei_; ///< The scalar indices of the referenced block
DenseIndex densej_; ///< The scalar indices of the referenced block
DenseIndex denseRows_; ///< The scalar size of the referenced block
DenseIndex denseCols_; ///< The scalar size of the referenced block
enum BlockType { Plain, SelfAdjoint, Transposed } blockType_; ///< The type of the referenced block, as determined by the block position
typedef SymmetricBlockMatrixBlockExpr<SymmetricBlockMatrixType> This;
public:
// Typedefs and constants used in Eigen
typedef typename const_selector<SymmetricBlockMatrixType, SymmetricBlockMatrix,
typename Eigen::internal::traits<This>::Scalar&, typename Eigen::internal::traits<This>::Scalar>::type ScalarRef;
typedef typename Eigen::internal::traits<This>::Scalar Scalar;
typedef typename Eigen::internal::traits<This>::Index Index;
static const Index ColsAtCompileTime = Eigen::Dynamic;
static const Index RowsAtCompileTime = Eigen::Dynamic;
typedef typename const_selector<SymmetricBlockMatrixType, SymmetricBlockMatrix, Matrix, const Matrix>::type
DenseMatrixType;
typedef Eigen::Map<DenseMatrixType, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> > OffDiagonal;
typedef Eigen::SelfAdjointView<Eigen::Block<DenseMatrixType>, Eigen::Upper> SelfAdjointView;
typedef Eigen::TriangularView<Eigen::Block<DenseMatrixType>, Eigen::Upper> TriangularView;
protected:
mutable Eigen::Block<DenseMatrixType> myBlock_;
template<typename OtherSymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr;
public:
/// Create a SymmetricBlockMatrixBlockExpr from the specified block of a SymmetricBlockMatrix.
SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index iBlock, Index jBlock) :
xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0))
{
initIndices(iBlock, jBlock);
}
/// Create a SymmetricBlockMatrixBlockExpr from the specified range of blocks of a
/// SymmetricBlockMatrix.
SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix,
Index firstRowBlock, Index firstColBlock, Index rowBlocks, Index colBlocks) :
xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0))
{
initIndices(firstRowBlock, firstColBlock, rowBlocks, colBlocks);
}
/// Create a SymmetricBlockMatrixBlockExpr from the specified range of blocks of a
/// SymmetricBlockMatrix.
SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index firstBlock, Index blocks, char dummy) :
xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0))
{
initIndices(firstBlock, firstBlock, blocks, blocks);
}
inline Index rows() const { return blockType_ != Transposed ? denseRows_ : denseCols_; }
inline Index cols() const { return blockType_ != Transposed ? denseCols_ : denseRows_; }
inline BlockType blockType() const { return blockType_; }
inline ScalarRef operator()(Index row, Index col) const
{
return coeffInternal<ScalarRef>(row, col);
}
inline OffDiagonal knownOffDiagonal() const
{
typedef Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> DynamicStride;
// We can return a Map if we are either on an off-diagonal block, or a block of size 0 or 1
assert(blockType_ != SelfAdjoint || (denseRows_ <= 1 && denseCols_ <= 1));
if(blockType_ == Transposed)
{
// Swap the inner and outer stride to produce a transposed Map
Eigen::Block<DenseMatrixType> block = const_cast<This&>(*this).xpr_.matrix_.block(densei_, densej_, denseRows_, denseCols_);
return Eigen::Map<DenseMatrixType, 0, DynamicStride>(block.data(), block.cols(), block.rows(),
DynamicStride(block.innerStride(), block.outerStride()));
}
else
{
Eigen::Block<DenseMatrixType> block = const_cast<This&>(*this).xpr_.matrix_.block(densei_, densej_, denseRows_, denseCols_);
return Eigen::Map<DenseMatrixType, 0, DynamicStride>(block.data(), block.rows(), block.cols(),
DynamicStride(block.outerStride(), block.innerStride()));
}
}
inline SelfAdjointView selfadjointView() const
{
assert(blockType_ == SelfAdjoint);
return myBlock_;
}
inline TriangularView triangularView() const
{
assert(blockType_ == SelfAdjoint);
return myBlock_;
}
template<typename Dest> inline void evalTo(Dest& dst) const
{
// Just try to assign to the object using either a selfadjoint view or a block view
if(blockType_ == SelfAdjoint)
dst = selfadjointView();
else if(blockType_ == Plain)
dst = myBlock_;
else
dst = myBlock_.transpose();
}
//template<typename MatrixType> inline void evalTo(const Eigen::SelfAdjointView<MatrixType, Eigen::Upper>& rhs) const
//{
// if(blockType_ == SelfAdjoint)
// rhs.nestedExpression().triangularView<Eigen::Upper>() = triangularView();
// else
// throw std::invalid_argument("Cannot assign an off-diagonal block to a self-adjoint matrix");
//}
//template<typename MatrixType> inline void evalTo(const Eigen::TriangularView<MatrixType, Eigen::Upper>& rhs) const
//{
// if(blockType_ == SelfAdjoint)
// rhs.nestedExpression().triangularView<Eigen::Upper>() = triangularView();
// else
// throw std::invalid_argument("Cannot assign an off-diagonal block to a self-adjoint matrix");
//}
template<typename RhsDerived>
This& operator=(const Eigen::MatrixBase<RhsDerived>& rhs)
{
// Just try to assign to the object using either a selfadjoint view or a block view
if(blockType_ == SelfAdjoint)
triangularView() = rhs.derived().template triangularView<Eigen::Upper>();
else if(blockType_ == Plain)
myBlock_ = rhs.derived();
else
myBlock_.transpose() = rhs.derived();
return *this;
}
template<typename MatrixType>
This& operator=(const Eigen::SelfAdjointView<MatrixType, Eigen::Upper>& rhs)
{
if(blockType_ == SelfAdjoint)
triangularView() = rhs.nestedExpression().template triangularView<Eigen::Upper>();
else
throw std::invalid_argument("Cannot assign a self-adjoint matrix to an off-diagonal block");
return *this;
}
template<typename OtherSymmetricBlockMatrixType>
This& operator=(const SymmetricBlockMatrixBlockExpr<OtherSymmetricBlockMatrixType>& other)
{
_doAssign(other);
return *this;
}
This& operator=(const This& other)
{
// This version is required so GCC doesn't synthesize a default operator=.
_doAssign(other);
return *this;
}
template<typename OtherSymmetricBlockMatrixType>
This& operator+=(const SymmetricBlockMatrixBlockExpr<OtherSymmetricBlockMatrixType>& other)
{
if(blockType_ == SelfAdjoint)
{
assert(other.blockType() == SelfAdjoint);
triangularView() += other.triangularView().nestedExpression();
}
else if(blockType_ == Plain)
{
assert(other.blockType() == Plain || other.blockType() == Transposed);
if(other.blockType() == Transposed)
myBlock_ += other.myBlock_.transpose();
else
myBlock_ += other.myBlock_;
}
else
{
assert(other.blockType() == Plain || other.blockType() == Transposed);
if(other.blockType() == Transposed)
myBlock_.transpose() += other.myBlock_.transpose();
else
myBlock_.transpose() += other.myBlock_;
}
return *this;
}
private:
void initIndices(Index iBlock, Index jBlock, Index blockRows = 1, Index blockCols = 1)
{
if(iBlock == jBlock && blockRows == blockCols)
{
densei_ = xpr_.offset(iBlock);
densej_ = densei_;
if(blockRows > 0)
xpr_.checkBlock(iBlock + blockRows - 1);
denseRows_ = xpr_.offsetUnchecked(iBlock + blockRows) - densei_;
if(blockCols > 0)
xpr_.checkBlock(jBlock + blockCols - 1);
denseCols_ = xpr_.offsetUnchecked(jBlock + blockCols) - densej_;
blockType_ = SelfAdjoint;
}
else
{
if(jBlock > iBlock || (iBlock == jBlock && blockCols > blockRows))
{
densei_ = xpr_.offset(iBlock);
densej_ = xpr_.offset(jBlock);
if(blockRows > 0)
xpr_.checkBlock(iBlock + blockRows - 1);
denseRows_ = xpr_.offsetUnchecked(iBlock + blockRows) - densei_;
if(blockCols > 0)
xpr_.checkBlock(jBlock + blockCols - 1);
denseCols_ = xpr_.offsetUnchecked(jBlock + blockCols) - densej_;
blockType_ = Plain;
}
else
{
densei_ = xpr_.offset(jBlock);
densej_ = xpr_.offset(iBlock);
if(blockCols > 0)
xpr_.checkBlock(jBlock + blockCols - 1);
denseRows_ = xpr_.offsetUnchecked(jBlock + blockCols) - densei_;
if(blockRows > 0)
xpr_.checkBlock(iBlock + blockRows - 1);
denseCols_ = xpr_.offsetUnchecked(iBlock + blockRows) - densej_;
blockType_ = Transposed;
}
// Validate that the block does not cross below the diagonal (the indices have already been
// flipped above the diagonal for ranges starting below the diagonal).
if(densei_ + denseRows_ > densej_ + 1)
throw std::invalid_argument("Off-diagonal block ranges may not cross the diagonal");
}
new (&myBlock_) Eigen::Block<DenseMatrixType>(xpr_.matrix_.block(densei_, densej_, denseRows_, denseCols_));
}
template<typename ScalarType>
inline ScalarType coeffInternal(Index row, Index col) const
{
// We leave index checking up to the Block class
if(blockType_ == Plain)
{
return myBlock_(row, col);
}
else if(blockType_ == SelfAdjoint)
{
if(row <= col)
return myBlock_(row, col);
else
return myBlock_.transpose()(row, col);
}
else
{
return myBlock_.transpose()(row, col);
}
}
template<typename OtherSymmetricBlockMatrixType>
void _doAssign(const SymmetricBlockMatrixBlockExpr<OtherSymmetricBlockMatrixType>& other)
{
if(blockType_ == SelfAdjoint)
{
assert(other.blockType() == SelfAdjoint);
triangularView() = other.triangularView().nestedExpression();
}
else if(blockType_ == Plain)
{
assert(other.blockType() == Plain || other.blockType() == Transposed);
if(other.blockType() == Transposed)
myBlock_ = other.myBlock_.transpose();
else
myBlock_ = other.myBlock_;
}
else
{
assert(other.blockType() == Plain || other.blockType() == Transposed);
if(other.blockType() == Transposed)
myBlock_.transpose() = other.myBlock_.transpose();
else
myBlock_.transpose() = other.myBlock_;
}
}
};
}

188
gtsam/base/tests/testSymmetricBlockMatrix.cpp Normal file
View File

@ -0,0 +1,188 @@
/* ----------------------------------------------------------------------------
* 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 testSymmetricBlockMatrix.cpp
* @brief Unit tests for SymmetricBlockMatrix class
* @author Richard Roberts
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <boost/assign/list_of.hpp>
using namespace std;
using namespace gtsam;
using boost::assign::list_of;
static SymmetricBlockMatrix testBlockMatrix(
list_of(3)(2)(1),
(Mat(6, 6) <<
1, 2, 3, 4, 5, 6,
2, 8, 9, 10, 11, 12,
3, 9, 15, 16, 17, 18,
4, 10, 16, 22, 23, 24,
5, 11, 17, 23, 29, 30,
6, 12, 18, 24, 30, 36));
/* ************************************************************************* */
TEST(SymmetricBlockMatrix, ReadBlocks)
{
// On the diagonal
Matrix expected1 = (Mat(2, 2) <<
22, 23,
23, 29);
Matrix actual1 = testBlockMatrix(1, 1);
// Test only writing the upper triangle for efficiency
Matrix actual1t = Matrix::Zero(2, 2);
actual1t.triangularView<Eigen::Upper>() = testBlockMatrix(1, 1).triangularView();
EXPECT(assert_equal(expected1, actual1));
EXPECT(assert_equal(Matrix(expected1.triangularView<Eigen::Upper>()), actual1t));
// Above the diagonal
Matrix expected2 = (Mat(3, 2) <<
4, 5,
10, 11,
16, 17);
Matrix actual2 = testBlockMatrix(0, 1);
EXPECT(assert_equal(expected2, actual2));
// Below the diagonal
Matrix expected3 = (Mat(2, 3) <<
4, 10, 16,
5, 11, 17);
Matrix actual3 = testBlockMatrix(1, 0);
EXPECT(assert_equal(expected3, actual3));
}
/* ************************************************************************* */
TEST(SymmetricBlockMatrix, WriteBlocks)
{
// On the diagonal
Matrix expected1 = testBlockMatrix(1, 1);
SymmetricBlockMatrix bm1 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix);
bm1(1, 1) = expected1.selfadjointView<Eigen::Upper>(); // Verified with debugger that this only writes the upper triangle
Matrix actual1 = bm1(1, 1);
EXPECT(assert_equal(expected1, actual1));
// On the diagonal
Matrix expected1p = testBlockMatrix(1, 1);
SymmetricBlockMatrix bm1p = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix);
bm1p(1, 1) = expected1p; // Verified with debugger that this only writes the upper triangle
Matrix actual1p = bm1p(1, 1);
EXPECT(assert_equal(expected1p, actual1p));
// Above the diagonal
Matrix expected2 = testBlockMatrix(0, 1);
SymmetricBlockMatrix bm2 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix);
bm2(0, 1) = expected2;
Matrix actual2 = bm2(0, 1);
EXPECT(assert_equal(expected2, actual2));
// Below the diagonal
Matrix expected3 = testBlockMatrix(1, 0);
SymmetricBlockMatrix bm3 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix);
bm3(1, 0) = expected3;
Matrix actual3 = bm3(1, 0);
EXPECT(assert_equal(expected3, actual3));
}
/* ************************************************************************* */
TEST(SymmetricBlockMatrix, Ranges)
{
// On the diagonal
Matrix expected1 = (Mat(3, 3) <<
22, 23, 24,
23, 29, 30,
24, 30, 36);
Matrix actual1 = testBlockMatrix.range(1, 3, 1, 3).selfadjointView();
Matrix actual1a = testBlockMatrix.range(1, 3, 1, 3);
EXPECT(assert_equal(expected1, actual1));
EXPECT(assert_equal(expected1, actual1a));
// Above the diagonal
Matrix expected2 = (Mat(3, 1) <<
24,
30,
36);
Matrix actual2 = testBlockMatrix.range(1, 3, 2, 3).knownOffDiagonal();
Matrix actual2a = testBlockMatrix.range(1, 3, 2, 3);
EXPECT(assert_equal(expected2, actual2));
EXPECT(assert_equal(expected2, actual2a));
// Below the diagonal
Matrix expected3 = (Mat(3, 3) <<
4, 10, 16,
5, 11, 17,
6, 12, 18);
Matrix actual3 = testBlockMatrix.range(1, 3, 0, 1).knownOffDiagonal();
Matrix actual3a = testBlockMatrix.range(1, 3, 0, 1);
EXPECT(assert_equal(expected3, actual3));
EXPECT(assert_equal(expected3, actual3a));
}
/* ************************************************************************* */
TEST(SymmetricBlockMatrix, expressions)
{
SymmetricBlockMatrix expected1(list_of(2)(3)(1), (Mat(6, 6) <<
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 4, 6, 8, 0,
0, 0, 0, 9, 12, 0,
0, 0, 0, 0, 16, 0,
0, 0, 0, 0, 0, 0));
SymmetricBlockMatrix expected2(list_of(2)(3)(1), (Mat(6, 6) <<
0, 0, 10, 15, 20, 0,
0, 0, 12, 18, 24, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0));
Matrix a = (Mat(1, 3) << 2, 3, 4);
Matrix b = (Mat(1, 2) << 5, 6);
SymmetricBlockMatrix bm1(list_of(2)(3)(1));
bm1.full().triangularView().setZero();
bm1(1, 1).selfadjointView().rankUpdate(a.transpose());
EXPECT(assert_equal(Matrix(expected1.full().selfadjointView()), bm1.full().selfadjointView()));
SymmetricBlockMatrix bm2(list_of(2)(3)(1));
bm2.full().triangularView().setZero();
bm2(0, 1).knownOffDiagonal() += b.transpose() * a;
EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm2.full().selfadjointView()));
SymmetricBlockMatrix bm3(list_of(2)(3)(1));
bm3.full().triangularView().setZero();
bm3(1, 0).knownOffDiagonal() += a.transpose() * b;
EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm3.full().selfadjointView()));
SymmetricBlockMatrix bm4(list_of(2)(3)(1));
bm4.full().triangularView().setZero();
bm4(1, 1) += expected1(1, 1);
EXPECT(assert_equal(Matrix(expected1.full().selfadjointView()), bm4.full().selfadjointView()));
SymmetricBlockMatrix bm5(list_of(2)(3)(1));
bm5.full().triangularView().setZero();
bm5(0, 1) += expected2(0, 1);
EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm5.full().selfadjointView()));
SymmetricBlockMatrix bm6(list_of(2)(3)(1));
bm6.full().triangularView().setZero();
bm6(1, 0) += expected2(1, 0);
EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm6.full().selfadjointView()));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

85
gtsam/linear/HessianFactor.cpp
View File

@ -129,8 +129,8 @@ HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) :
if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument(
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
info_(0,0) = Sigma.inverse(); // G
info_(0,1) = info_(0,0) * mu; // g
info_(1,1)(0,0) = mu.dot(info_(0,1).col(0)); // f
info_(0,1) = info_(0,0).selfadjointView() * mu; // g
info_(1,1)(0,0) = mu.dot(info_(0,1).knownOffDiagonal().col(0)); // f
}
/* ************************************************************************* */
@ -230,11 +230,11 @@ void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info)
{
if(jf.get_model()->isConstrained())
throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model");
info.full().noalias() = jf.matrixObject().full().transpose() *
info.full().triangularView() = jf.matrixObject().full().transpose() *
(jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() *
jf.matrixObject().full();
} else {
info.full().noalias() = jf.matrixObject().full().transpose() * jf.matrixObject().full();
info.full().triangularView() = jf.matrixObject().full().transpose() * jf.matrixObject().full();
}
}
}
@ -292,7 +292,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
}
dims.back() = 1;
info_ = SymmetricBlockMatrix(dims);
info_.full().setZero();
info_.full().triangularView().setZero();
gttoc(allocate);
// Form A' * A
@ -318,7 +318,7 @@ void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) c
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>()), "Augmented information matrix: ");
gtsam::print(Matrix(info_.full().selfadjointView()), "Augmented information matrix: ");
}
/* ************************************************************************* */
@ -328,9 +328,9 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
else {
if(!Factor::equals(lf, tol))
return false;
Matrix thisMatrix = this->info_.full().selfadjointView<Eigen::Upper>();
Matrix thisMatrix = this->info_.full().selfadjointView();
thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
Matrix rhsMatrix = static_cast<const HessianFactor&>(lf).info_.full().selfadjointView<Eigen::Upper>();
Matrix rhsMatrix = static_cast<const HessianFactor&>(lf).info_.full().selfadjointView();
rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;
return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
}
@ -339,13 +339,13 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
/* ************************************************************************* */
Matrix HessianFactor::augmentedInformation() const
{
return info_.full().selfadjointView<Eigen::Upper>();
return info_.full().selfadjointView();
}
/* ************************************************************************* */
Matrix HessianFactor::information() const
{
return info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>();
return info_.range(0, this->size(), 0, this->size()).selfadjointView();
}
/* ************************************************************************* */
@ -369,7 +369,7 @@ double HessianFactor::error(const VectorValues& c) const {
// 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;
xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x;
return 0.5 * (f - 2.0 * xtg + xGx);
}
@ -397,12 +397,7 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
DenseIndex slot2 = (j2 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j2];
for(DenseIndex j1=0; j1<=j2; ++j1) {
DenseIndex slot1 = (j1 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j1];
if(slot2 > slot1)
info_(slot1, slot2).noalias() += update.info_(j1, j2);
else if(slot1 > slot2)
info_(slot2, slot1).noalias() += update.info_(j1, j2).transpose();
else
info_(slot1, slot2).triangularView<Eigen::Upper>() += update.info_(j1, j2);
info_(slot1, slot2) += update.info_(j1, j2);
}
}
gttoc(update);
@ -458,48 +453,21 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt
for(DenseIndex j1 = 0; j1 <= j2; ++j1) { // Vertical block of Hessian
DenseIndex slot1 = (j1 == update.size()) ? nrInfoBlocks-1 : slots[j1];
assert(slot1 >= 0 && slot1 < nrInfoBlocks);
updateBlocks.offset(0);
if(slot2 > slot1)
info_(slot1, slot2).noalias() += updateBlocks(j1).transpose() * updateBlocks(j2);
else if(slot1 > slot2)
info_(slot2, slot1).noalias() += updateBlocks(j2).transpose() * updateBlocks(j1);
if(slot1 != slot2)
info_(slot1, slot2).knownOffDiagonal() += updateBlocks(j1).transpose() * updateBlocks(j2);
else
info_(slot1, slot2).triangularView<Eigen::Upper>() += updateBlocks(j1).transpose() * updateBlocks(j2);
info_(slot1, slot2).selfadjointView().rankUpdate(updateBlocks(j1).transpose());
}
}
gttoc(update);
}
}
/* ************************************************************************* */
GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFrontals)
{
gttic(HessianFactor_splitEliminatedFactor);
// Create one big conditionals with many frontal variables.
gttic(Construct_conditional);
const size_t varDim = info_.offset(nrFrontals);
VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(info_, varDim);
Ab.full() = info_.range(0, nrFrontals, 0, info_.nBlocks());
GaussianConditional::shared_ptr conditional = boost::make_shared<GaussianConditional>(
keys_, nrFrontals, Ab);
gttoc(Construct_conditional);
gttic(Remaining_factor);
// Take lower-right block of Ab_ to get the new factor
info_.blockStart() = nrFrontals;
// Assign the keys
keys_.erase(begin(), begin() + nrFrontals);
gttoc(Remaining_factor);
return conditional;
}
/* ************************************************************************* */
GaussianFactor::shared_ptr HessianFactor::negate() const
{
shared_ptr result = boost::make_shared<This>(*this);
result->info_.full() = -result->info_.full(); // Negate the information matrix of the result
result->info_.full().triangularView() = -result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result
return result;
}
@ -521,12 +489,12 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
Vector xj = x.at(keys_[j]);
DenseIndex i = 0;
for (; i < j; ++i)
y[i] += info_(i, j) * xj;
y[i] += info_(i, j).knownOffDiagonal() * xj;
// blocks on the diagonal are only half
y[i] += info_(j, j).selfadjointView<Eigen::Upper>() * xj;
y[i] += info_(j, j).selfadjointView() * xj;
// for below diagonal, we take transpose block from upper triangular part
for (i = j + 1; i < (DenseIndex)size(); ++i)
y[i] += info_(j, i).transpose() * xj;
y[i] += info_(i, j).knownOffDiagonal() * xj;
}
// copy to yvalues
@ -546,7 +514,7 @@ VectorValues HessianFactor::gradientAtZero() const {
VectorValues g;
size_t n = size();
for (size_t j = 0; j < n; ++j)
g.insert(keys_[j],-info_(j,n));
g.insert(keys_[j], -info_(j,n).knownOffDiagonal());
return g;
}
@ -567,12 +535,17 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
}
// Do dense elimination
if(!choleskyPartial(jointFactor->info_.matrix(), jointFactor->info_.offset(keys.size())))
GaussianConditional::shared_ptr conditional;
try {
VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(keys.size());
conditional = boost::make_shared<GaussianConditional>(jointFactor->keys(), keys.size(), Ab);
// Erase the eliminated keys in the remaining factor
jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + keys.size());
} catch(CholeskyFailed&) {
throw IndeterminantLinearSystemException(keys.front());
}
// Split conditional
GaussianConditional::shared_ptr conditional = jointFactor->splitEliminatedFactor(keys.size());
// Return result
return make_pair(conditional, jointFactor);
}

19
gtsam/linear/HessianFactor.h
View File

@ -265,13 +265,13 @@ namespace gtsam {
* 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(); }
SymmetricBlockMatrix::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(); }
SymmetricBlockMatrix::Block info() { return info_.full(); }
/** Return the constant term \f$ f \f$ as described above
* @return The constant term \f$ f \f$
@ -287,21 +287,25 @@ namespace gtsam {
* @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$ */
constBlock::ColXpr linearTerm(const_iterator j) const { return info_(j-begin(), size()).col(0); }
constBlock::OffDiagonal::ColXpr linearTerm(const_iterator j) const {
return info_(j-begin(), size()).knownOffDiagonal().col(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$ */
Block::ColXpr linearTerm(iterator j) { return info_(j-begin(), size()).col(0); }
Block::OffDiagonal::ColXpr linearTerm(iterator j) {
return info_(j-begin(), size()).knownOffDiagonal().col(0); }
/** Return the complete linear term \f$ g \f$ as described above.
* @return The linear term \f$ g \f$ */
constBlock::ColXpr linearTerm() const { return info_.range(0, this->size(), this->size(), this->size() + 1).col(0); }
constBlock::OffDiagonal::ColXpr linearTerm() const {
return info_.range(0, this->size(), this->size(), this->size() + 1).knownOffDiagonal().col(0); }
/** Return the complete linear term \f$ g \f$ as described above.
* @return The linear term \f$ g \f$ */
Block::ColXpr linearTerm() { return info_.range(0, this->size(), this->size(), this->size() + 1).col(0); }
Block::OffDiagonal::ColXpr linearTerm() {
return info_.range(0, this->size(), this->size(), this->size() + 1).knownOffDiagonal().col(0); }
/** Return the augmented information matrix represented by this GaussianFactor.
* The augmented information matrix contains the information matrix with an
@ -401,9 +405,6 @@ namespace gtsam {
private:
/** split partially eliminated factor */
boost::shared_ptr<GaussianConditional> splitEliminatedFactor(size_t nrFrontals);
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>

15
gtsam/linear/tests/testHessianFactorUnordered.cpp
View File

@ -86,7 +86,7 @@ TEST(HessianFactor, ConversionConstructor)
/* ************************************************************************* */
TEST(HessianFactor, Constructor1)
{
Matrix G = (Mat(2,2) << 3.0, 5.0, 0.0, 6.0);
Matrix G = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0);
Vector g = (Vec(2) << -8.0, -9.0);
double f = 10.0;
HessianFactor factor(0, G, g, f);
@ -132,7 +132,7 @@ TEST(HessianFactor, Constructor2)
{
Matrix G11 = (Mat(1,1) << 1.0);
Matrix G12 = (Mat(1,2) << 2.0, 4.0);
Matrix G22 = (Mat(2,2) << 3.0, 5.0, 0.0, 6.0);
Matrix G22 = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0);
Vector g1 = (Vec(1) << -7.0);
Vector g2 = (Vec(2) << -8.0, -9.0);
double f = 10.0;
@ -175,10 +175,10 @@ TEST(HessianFactor, Constructor3)
Matrix G12 = (Mat(1,2) << 2.0, 4.0);
Matrix G13 = (Mat(1,3) << 3.0, 6.0, 9.0);
Matrix G22 = (Mat(2,2) << 3.0, 5.0, 0.0, 6.0);
Matrix G22 = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0);
Matrix G23 = (Mat(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0);
Matrix G33 = (Mat(3,3) << 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0);
Matrix G33 = (Mat(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0);
Vector g1 = (Vec(1) << -7.0);
Vector g2 = (Vec(2) << -8.0, -9.0);
@ -222,10 +222,10 @@ TEST(HessianFactor, ConstructorNWay)
Matrix G12 = (Mat(1,2) << 2.0, 4.0);
Matrix G13 = (Mat(1,3) << 3.0, 6.0, 9.0);
Matrix G22 = (Mat(2,2) << 3.0, 5.0, 0.0, 6.0);
Matrix G22 = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0);
Matrix G23 = (Mat(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0);
Matrix G33 = (Mat(3,3) << 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0);
Matrix G33 = (Mat(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0);
Vector g1 = (Vec(1) << -7.0);
Vector g2 = (Vec(2) << -8.0, -9.0);
@ -423,8 +423,7 @@ TEST(HessianFactor, combine) {
-100.0000, 0.0, 20.0000, 0.0, 80.0000, 0.0, -20.0000,
0.0, -100.0000, 0.0, 20.0000, 0.0, 80.0000, 14.0000,
25.0000, -17.5000, -5.0000, 3.5000, -20.0000, 14.0000, 7.4500);
EXPECT(assert_equal(Matrix(expected.triangularView<Eigen::Upper>()),
Matrix(actual.matrixObject().full().triangularView<Eigen::Upper>()), tol));
EXPECT(assert_equal(expected, Matrix(actual.matrixObject().full()), tol));
}

5
gtsam/nonlinear/LinearContainerFactor.cpp
View File

@ -123,10 +123,9 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con
jacFactor->getb() = -jacFactor->unweighted_error(delta);
} else {
HessianFactor::shared_ptr hesFactor = boost::dynamic_pointer_cast<HessianFactor>(linFactor);
size_t dim = hesFactor->linearTerm().size();
Eigen::Block<HessianFactor::Block> Gview = hesFactor->info().block(0, 0, dim, dim);
SymmetricBlockMatrix::constBlock Gview = hesFactor->matrixObject().range(0, hesFactor->size(), 0, hesFactor->size());
Vector deltaVector = delta.vector(keys());
Vector G_delta = Gview.selfadjointView<Eigen::Upper>() * deltaVector;
Vector G_delta = Gview.selfadjointView() * deltaVector;
hesFactor->constantTerm() += deltaVector.dot(G_delta) - 2.0 * deltaVector.dot(hesFactor->linearTerm());
hesFactor->linearTerm() -= G_delta;
}

4
gtsam/nonlinear/Marginals.cpp
View File

@ -76,7 +76,9 @@ Matrix Marginals::marginalInformation(Key variable) const {
/* ************************************************************************* */
JointMarginal Marginals::jointMarginalCovariance(const std::vector<Key>& variables) const {
JointMarginal info = jointMarginalInformation(variables);
info.blockMatrix_.full() = info.blockMatrix_.full().inverse();
info.blockMatrix_.full().triangularView() =
info.blockMatrix_.full().selfadjointView().llt().solve(
Matrix::Identity(info.blockMatrix_.full().rows(), info.blockMatrix_.full().rows())).triangularView<Eigen::Upper>();
return info;
}

2
gtsam/nonlinear/Marginals.h
View File

@ -116,7 +116,7 @@ public:
* in scope while this view is needed. Otherwise assign this block object to a Matrix
* to store it.
*/
const Matrix& fullMatrix() const { return blockMatrix_.matrix(); }
Eigen::SelfAdjointView<const Matrix, Eigen::Upper> fullMatrix() const { return blockMatrix_.matrix(); }
/** Print */
void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;

2
gtsam/slam/tests/testAntiFactor.cpp
View File

@ -69,7 +69,7 @@ TEST( AntiFactor, NegativeHessian)
size_t variable_count = originalFactor->size();
for(size_t i = 0; i < variable_count; ++i){
for(size_t j = i; j < variable_count; ++j){
Matrix expected_G = -originalHessian->info(originalHessian->begin()+i, originalHessian->begin()+j);
Matrix expected_G = -Matrix(originalHessian->info(originalHessian->begin()+i, originalHessian->begin()+j));
Matrix actual_G = antiHessian->info(antiHessian->begin()+i, antiHessian->begin()+j);
CHECK(assert_equal(expected_G, actual_G, 1e-5));
}

14
gtsam_unstable/nonlinear/LinearizedFactor.cpp
View File

@ -153,7 +153,7 @@ LinearizedHessianFactor::LinearizedHessianFactor(
// Update the BlockInfo accessor
info_ = SymmetricBlockMatrix(dims, dims+hessian->size()+1);
// Copy the augmented matrix holding G, g, and f
info_.matrix() = hessian->info();
info_.full() = hessian->info();
}
/* ************************************************************************* */
@ -166,7 +166,7 @@ void LinearizedHessianFactor::print(const std::string& s, const KeyFormatter& ke
std::cout << keyFormatter(key) << " ";
std::cout << std::endl;
gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView<Eigen::Upper>()), "Ab^T * Ab: ");
gtsam::print(Matrix(info_.full()), "Ab^T * Ab: ");
lin_points_.print("Linearization Point: ");
}
@ -177,9 +177,9 @@ bool LinearizedHessianFactor::equals(const NonlinearFactor& expected, double tol
const This *e = dynamic_cast<const This*> (&expected);
if (e) {
Matrix thisMatrix = this->info_.full().selfadjointView<Eigen::Upper>();
Matrix thisMatrix = this->info_.full();
thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
Matrix rhsMatrix = e->info_.full().selfadjointView<Eigen::Upper>();
Matrix rhsMatrix = e->info_.full();
rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;
return Base::equals(expected, tol)
@ -207,7 +207,7 @@ double LinearizedHessianFactor::error(const Values& c) const {
// error 0.5*(f - 2*x'*g + x'*G*x)
double f = constantTerm();
double xtg = dx.dot(linearTerm());
double xGx = dx.transpose() * squaredTerm().selfadjointView<Eigen::Upper>() * dx;
double xGx = dx.transpose() * squaredTerm() * dx;
return 0.5 * (f - 2.0 * xtg + xGx);
}
@ -229,11 +229,11 @@ LinearizedHessianFactor::linearize(const Values& c) const {
// f2 = f1 - 2*dx'*g1 + dx'*G1*dx
//newInfo(this->size(), this->size())(0,0) += -2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView<Eigen::Upper>() * dx;
double f = constantTerm() - 2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView<Eigen::Upper>() * dx;
double f = constantTerm() - 2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm() * dx;
// g2 = g1 - G1*dx
//newInfo.rangeColumn(0, this->size(), this->size(), 0) -= squaredTerm().selfadjointView<Eigen::Upper>() * dx;
Vector g = linearTerm() - squaredTerm().selfadjointView<Eigen::Upper>() * dx;
Vector g = linearTerm() - squaredTerm() * dx;
std::vector<Vector> gs;
for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) {
gs.push_back(g.segment(info_.offset(i), info_.offset(i+1) - info_.offset(i)));

10
gtsam_unstable/nonlinear/LinearizedFactor.h
View File

@ -176,8 +176,8 @@ public:
/** hessian block data types */
typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
typedef SymmetricBlockMatrix::constBlock constBlock; ///< A block from the Hessian matrix (const version)
typedef SymmetricBlockMatrix::Block::ColXpr Column; ///< A column containing the linear term h
typedef SymmetricBlockMatrix::constBlock::ColXpr constColumn; ///< A column containing the linear term h (const version)
typedef SymmetricBlockMatrix::Block::OffDiagonal::ColXpr Column; ///< A column containing the linear term h
typedef SymmetricBlockMatrix::constBlock::OffDiagonal::ColXpr constColumn; ///< A column containing the linear term h (const version)
protected:
@ -220,11 +220,11 @@ public:
* @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_(j - this->begin(), this->size()).col(0); }
constColumn linearTerm(const_iterator j) const { return info_(j - this->begin(), this->size()).knownOffDiagonal().col(0); }
/** Return the complete linear term \f$ g \f$ as described above.
* @return The linear term \f$ g \f$ */
constColumn linearTerm() const { return info_.range(0, this->size(), this->size(), this->size() + 1).col(0); };
constColumn linearTerm() const { return info_.range(0, this->size(), this->size(), this->size() + 1).knownOffDiagonal().col(0); };
/** Return a view of the block at (j1,j2) of the <emph>upper-triangular part</emph> of the
* squared term \f$ H \f$, no data is copied. See HessianFactor class documentation
@ -242,7 +242,7 @@ public:
* 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 squaredTerm() const { return info_.range(0, this->size(), 0, this->size()); }
constBlock::SelfAdjointView squaredTerm() const { return info_.range(0, this->size(), 0, this->size()).selfadjointView(); }
/** get the dimension of the factor (number of rows on linearization) */