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

Cherry-picked Richard commit: Removed use of boost::range 'join' - replaced with a special flag to add one dimension in VerticalBlockMatrix and SymmetricBlockMatrix

release/4.3a0
dellaert 2014-03-02 00:07:12 -05:00
parent 15a69fa1ca
commit 744d9f7c1c
5 changed files with 428 additions and 603 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

376
gtsam/base/SymmetricBlockMatrix.h
View File

@ -1,20 +1,20 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file SymmetricBlockMatrix.h * @file SymmetricBlockMatrix.h
* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional. * @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional.
* @author Richard Roberts * @author Richard Roberts
* @date Sep 18, 2010 * @date Sep 18, 2010
*/ */
#pragma once #pragma once
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
@ -23,334 +23,215 @@
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
class VerticalBlockMatrix; class VerticalBlockMatrix;
/** /**
* This class stores a dense matrix and allows it to be accessed as a * This class stores a dense matrix and allows it to be accessed as a collection of blocks. When
* collection of blocks. When constructed, the caller must provide the * constructed, the caller must provide the dimensions of the blocks.
* dimensions of the blocks.
* *
* The block structure is symmetric, but the underlying matrix does not * The block structure is symmetric, but the underlying matrix does not necessarily need to be.
* necessarily need to be.
* *
* This class also has a parameter that can be changed after construction to * This class also has a parameter that can be changed after construction to change the apparent
* change the apparent matrix view. firstBlock() determines the block that * matrix view. firstBlock() determines the block that appears to have index 0 for all operations
* appears to have index 0 for all operations (except re-setting firstBlock). * (except re-setting firstBlock()).
* *
* @addtogroup base */ * @addtogroup base */
class GTSAM_EXPORT SymmetricBlockMatrix { class GTSAM_EXPORT SymmetricBlockMatrix
public: {
public:
typedef SymmetricBlockMatrix This; typedef SymmetricBlockMatrix This;
typedef SymmetricBlockMatrixBlockExpr<This> Block; typedef SymmetricBlockMatrixBlockExpr<This> Block;
typedef SymmetricBlockMatrixBlockExpr<const This> constBlock; typedef SymmetricBlockMatrixBlockExpr<const This> constBlock;
protected: protected:
Matrix matrix_; ///< The full matrix Matrix matrix_; ///< The full matrix
FastVector<DenseIndex> variableColOffsets_; ///< the starting columns of each block (0-based)
/// the starting columns of each block (0-based) DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment.
FastVector<DenseIndex> variableColOffsets_;
/// Changes apparent matrix view, see main class comment. public:
DenseIndex blockStart_;
public:
/// Construct from an empty matrix (asserts that the matrix is empty) /// Construct from an empty matrix (asserts that the matrix is empty)
SymmetricBlockMatrix() : SymmetricBlockMatrix() :
blockStart_(0) { blockStart_(0)
{
variableColOffsets_.push_back(0); variableColOffsets_.push_back(0);
assertInvariants(); assertInvariants();
} }
/// Construct from a container of the sizes of each block. /// Construct from a container of the sizes of each block.
template<typename CONTAINER> template<typename CONTAINER>
SymmetricBlockMatrix(const CONTAINER& dimensions) : SymmetricBlockMatrix(const CONTAINER& dimensions, bool appendOneDimension = false) :
blockStart_(0) { blockStart_(0)
fillOffsets(dimensions.begin(), dimensions.end()); {
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
assertInvariants(); assertInvariants();
} }
/// Construct from iterator over the sizes of each vertical block. /// Construct from iterator over the sizes of each vertical block.
template<typename ITERATOR> template<typename ITERATOR>
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim) : SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension = false) :
blockStart_(0) { blockStart_(0)
fillOffsets(firstBlockDim, lastBlockDim); {
fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
assertInvariants(); assertInvariants();
} }
/** /// Construct from a container of the sizes of each vertical block and a pre-prepared matrix.
* @brief Construct from a container of the sizes of each vertical block
* and a pre-prepared matrix.
*/
template<typename CONTAINER> template<typename CONTAINER>
SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) : SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
blockStart_(0) { blockStart_(0)
{
matrix_.resize(matrix.rows(), matrix.cols()); matrix_.resize(matrix.rows(), matrix.cols());
matrix_.triangularView<Eigen::Upper>() = matrix_.triangularView<Eigen::Upper>() = matrix.triangularView<Eigen::Upper>();
matrix.triangularView<Eigen::Upper>(); fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
fillOffsets(dimensions.begin(), dimensions.end()); if(matrix_.rows() != matrix_.cols())
if (matrix_.rows() != matrix_.cols()) throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix.");
throw std::invalid_argument("Requested to create a SymmetricBlockMatrix" if(variableColOffsets_.back() != matrix_.cols())
" from a non-square matrix."); throw std::invalid_argument("Requested to create a SymmetricBlockMatrix with dimensions that do not sum to the total size of the provided matrix.");
if (variableColOffsets_.back() != matrix_.cols())
throw std::invalid_argument(
"Requested to create a SymmetricBlockMatrix with dimensions "
"that do not sum to the total size of the provided matrix.");
assertInvariants(); assertInvariants();
} }
/** /// Copy the block structure, but do not copy the matrix data. If blockStart() has been
* Copy the block structure, but do not copy the matrix data. If blockStart() /// modified, this copies the structure of the corresponding matrix view. In the destination
* has been modified, this copies the structure of the corresponding matrix. /// SymmetricBlockMatrix, blockStart() will be 0.
* In the destination SymmetricBlockMatrix, blockStart() will be 0. static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& other);
*/
static SymmetricBlockMatrix LikeActiveViewOf(
const SymmetricBlockMatrix& other);
/** /// Copy the block structure, but do not copy the matrix data. If blockStart() has been
* Copy the block structure, but do not copy the matrix data. If blockStart() /// modified, this copies the structure of the corresponding matrix view. In the destination
* has been modified, this copies the structure of the corresponding matrix. /// SymmetricBlockMatrix, blockStart() will be 0.
* In the destination SymmetricBlockMatrix, blockStart() will be 0. static SymmetricBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& other);
*/
static SymmetricBlockMatrix LikeActiveViewOf(
const VerticalBlockMatrix& other);
/// Row size /// Row size
DenseIndex rows() const { DenseIndex rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
assertInvariants();
return variableColOffsets_.back() - variableColOffsets_[blockStart_];
}
/// Column size /// Column size
DenseIndex cols() const { DenseIndex cols() const { return rows(); }
return rows();
}
/// Block count /// Block count
DenseIndex nBlocks() const { DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
assertInvariants();
return variableColOffsets_.size() - 1 - blockStart_;
}
/** /// Access the block with vertical block index \c i_block and horizontal block index \c j_block.
* Access the block with vertical block index \c i_block and horizontal block /// Note that the actual block accessed in the underlying matrix is relative to blockStart().
* 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) { Block operator()(DenseIndex i_block, DenseIndex j_block) {
return Block(*this, i_block, j_block); return Block(*this, i_block, j_block);
} }
/** /// Access the block with vertical block index \c i_block and horizontal block index \c j_block.
* Access the block with vertical block index \c i_block and horizontal block /// Note that the actual block accessed in the underlying matrix is relative to blockStart().
* 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 { constBlock operator()(DenseIndex i_block, DenseIndex j_block) const {
return constBlock(*this, i_block, j_block); return constBlock(*this, i_block, j_block);
} }
/** /// Access the range of blocks starting with vertical block index \c i_startBlock, ending with
* Access the range of blocks starting with vertical block index /// vertical block index \c i_endBlock, starting with horizontal block index \c j_startBlock,
* \c i_startBlock, ending with vertical block index \c i_endBlock, starting /// and ending with horizontal block index \c j_endBlock. End block indices are exclusive. Note
* with horizontal block index \c j_startBlock, and ending with horizontal /// that the actual blocks accessed in the underlying matrix are relative to blockStart().
* block index \c j_endBlock. End block indices are exclusive. Note that the Block range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) {
* 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(); assertInvariants();
return Block(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock, return Block(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock, j_endBlock - j_startBlock);
j_endBlock - j_startBlock);
} }
/** /// Access the range of blocks starting with vertical block index \c i_startBlock, ending with
* Access the range of blocks starting with vertical block index /// vertical block index \c i_endBlock, starting with horizontal block index \c j_startBlock,
* \c i_startBlock, ending with vertical block index \c i_endBlock, starting /// and ending with horizontal block index \c j_endBlock. End block indices are exclusive. Note
* with horizontal block index \c j_startBlock, and ending with horizontal /// that the actual blocks accessed in the underlying matrix are relative to blockStart().
* block index \c j_endBlock. End block indices are exclusive. Note that the constBlock range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const {
* 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(); assertInvariants();
return constBlock(*this, i_startBlock, j_startBlock, return constBlock(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock, j_endBlock - j_startBlock);
i_endBlock - i_startBlock, j_endBlock - j_startBlock);
} }
/** /** Return the full matrix, *not* including any portions excluded by firstBlock(). */
* Return the full matrix, *not* including any portions excluded by Block full()
* firstBlock(). {
*/
Block full() {
return Block(*this, 0, nBlocks(), 0); return Block(*this, 0, nBlocks(), 0);
} }
/** /** Return the full matrix, *not* including any portions excluded by firstBlock(). */
* Return the full matrix, *not* including any portions excluded by constBlock full() const
* firstBlock(). {
*/
constBlock full() const {
return constBlock(*this, 0, nBlocks(), 0); return constBlock(*this, 0, nBlocks(), 0);
} }
/** /** Access to full matrix, including any portions excluded by firstBlock() to other operations. */
* Access to full matrix, including any portions excluded by firstBlock() Eigen::SelfAdjointView<const Matrix, Eigen::Upper> matrix() const
* to other operations. {
*/
Eigen::SelfAdjointView<const Matrix, Eigen::Upper> matrix() const {
return matrix_; return matrix_;
} }
/** Access to full matrix, including any portions excluded by firstBlock() /** Access to full matrix, including any portions excluded by firstBlock() to other operations. */
* to other operations. Eigen::SelfAdjointView<Matrix, Eigen::Upper> matrix()
*/ {
Eigen::SelfAdjointView<Matrix, Eigen::Upper> matrix() {
return matrix_; return matrix_;
} }
/** /// Return the absolute offset in the underlying matrix of the start of the specified \c block.
* Return the absolute offset in the underlying matrix DenseIndex offset(DenseIndex block) const
* of the start of the specified \c block. {
*/
DenseIndex offset(DenseIndex block) const {
assertInvariants(); assertInvariants();
DenseIndex actualBlock = block + blockStart_; DenseIndex actualBlock = block + blockStart_;
checkBlock(actualBlock); checkBlock(actualBlock);
return variableColOffsets_[actualBlock]; return variableColOffsets_[actualBlock];
} }
/** /// Retrieve or modify the first logical block, i.e. the block referenced by block index 0.
* Retrieve or modify the first logical block, i.e. the block referenced by /// Blocks before it will be inaccessible, except by accessing the underlying matrix using
* block index 0. Blocks before it will be inaccessible, except by accessing /// matrix().
* the underlying matrix using matrix(). DenseIndex& blockStart() { return blockStart_; }
*/
DenseIndex& blockStart() {
return blockStart_;
}
/** /// Retrieve the first logical block, i.e. the block referenced by block index 0. Blocks before
* Retrieve the first logical block, i.e. the block referenced by block index 0. /// it will be inaccessible, except by accessing the underlying matrix using matrix().
* Blocks before it will be inaccessible, except by accessing the underlying DenseIndex blockStart() const { return blockStart_; }
* matrix using matrix().
*/
DenseIndex blockStart() const {
return blockStart_;
}
/** /// Do partial Cholesky in-place and return the eliminated block matrix, leaving the remaining
* Do partial Cholesky in-place and return the eliminated block matrix, /// symmetric matrix in place.
* leaving the remaining symmetric matrix in place.
*/
VerticalBlockMatrix choleskyPartial(DenseIndex nFrontals); VerticalBlockMatrix choleskyPartial(DenseIndex nFrontals);
protected: protected:
void assertInvariants() const { void assertInvariants() const
{
assert(matrix_.rows() == matrix_.cols()); assert(matrix_.rows() == matrix_.cols());
assert(matrix_.cols() == variableColOffsets_.back()); assert(matrix_.cols() == variableColOffsets_.back());
assert(blockStart_ < (DenseIndex)variableColOffsets_.size()); assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
} }
void checkBlock(DenseIndex block) const { void checkBlock(DenseIndex block) const
{
assert(matrix_.rows() == matrix_.cols()); assert(matrix_.rows() == matrix_.cols());
assert(matrix_.cols() == variableColOffsets_.back()); assert(matrix_.cols() == variableColOffsets_.back());
assert(block >= 0); assert(block >= 0);
assert(block < (DenseIndex)variableColOffsets_.size()-1); assert(block < (DenseIndex)variableColOffsets_.size()-1);
assert( assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
} }
DenseIndex offsetUnchecked(DenseIndex block) const { DenseIndex offsetUnchecked(DenseIndex block) const
{
return variableColOffsets_[block + blockStart_]; 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> template<typename ITERATOR>
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) { void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension)
variableColOffsets_.resize((lastBlockDim - firstBlockDim) + 1); {
variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
variableColOffsets_[0] = 0; variableColOffsets_[0] = 0;
DenseIndex j = 0; DenseIndex j=0;
for (ITERATOR dim = firstBlockDim; dim != lastBlockDim; ++dim) { for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
variableColOffsets_[j + 1] = variableColOffsets_[j] + *dim; variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
++j; ++ j;
}
if(appendOneDimension)
{
variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
++ j;
} }
} }
friend class VerticalBlockMatrix; friend class VerticalBlockMatrix;
template<typename SymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr; template<typename SymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
@ -359,16 +240,15 @@ private:
ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
ar & BOOST_SERIALIZATION_NVP(blockStart_); ar & BOOST_SERIALIZATION_NVP(blockStart_);
} }
}; };
/* ************************************************************************* */ /* ************************************************************************* */
class CholeskyFailed: public gtsam::ThreadsafeException<CholeskyFailed> { class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
public: {
CholeskyFailed() throw () { public:
} CholeskyFailed() throw() {}
virtual ~CholeskyFailed() throw () { virtual ~CholeskyFailed() throw() {}
} };
};
} //\ namespace gtsam }

267
gtsam/base/VerticalBlockMatrix.h
View File

@ -22,148 +22,113 @@
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
class SymmetricBlockMatrix; class SymmetricBlockMatrix;
/** /**
* This class stores a dense matrix and allows it to be accessed as a collection * This class stores a dense matrix and allows it to be accessed as a collection of vertical
* of vertical blocks. * blocks. The dimensions of the blocks are provided when constructing this class.
* *
* The dimensions of the blocks are provided when constructing this class. * 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).
* *
* This class also has three parameters that can be changed after construction * @addtogroup base */
* that change the apparent view of the matrix without any reallocation or data class GTSAM_EXPORT VerticalBlockMatrix
* copying. firstBlock() determines the block that has index 0 for all operations {
* (except for re-setting firstBlock()). rowStart() determines the apparent public:
* 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 GTSAM_EXPORT VerticalBlockMatrix {
public:
typedef VerticalBlockMatrix This; typedef VerticalBlockMatrix This;
typedef Eigen::Block<Matrix> Block; typedef Eigen::Block<Matrix> Block;
typedef Eigen::Block<const Matrix> constBlock; typedef Eigen::Block<const Matrix> constBlock;
protected: protected:
Matrix matrix_; ///< The full matrix Matrix matrix_; ///< The full matrix
FastVector<DenseIndex> variableColOffsets_; ///< the starting columns of each block (0-based)
/// the starting columns of each block (0-based) DenseIndex rowStart_; ///< Changes apparent matrix view, see main class comment.
FastVector<DenseIndex> variableColOffsets_; DenseIndex rowEnd_; ///< Changes apparent matrix view, see main class comment.
DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment.
DenseIndex rowStart_; ///< Changes apparent matrix view, see class comments. public:
DenseIndex rowEnd_; ///< Changes apparent matrix view, see class comments.
DenseIndex blockStart_; ///< Changes apparent matrix view, see class comments.
#define ASSERT_INVARIANTS \
assert(matrix_.cols() == variableColOffsets_.back());\
assert(blockStart_ < (DenseIndex)variableColOffsets_.size());\
assert(rowStart_ <= matrix_.rows());\
assert(rowEnd_ <= matrix_.rows());\
assert(rowStart_ <= rowEnd_);\
#define CHECK_BLOCK(block) \
assert(matrix_.cols() == variableColOffsets_.back());\
assert(block < (DenseIndex)variableColOffsets_.size() - 1);\
assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
public:
/** Construct an empty VerticalBlockMatrix */ /** Construct an empty VerticalBlockMatrix */
VerticalBlockMatrix() : VerticalBlockMatrix() :
rowStart_(0), rowEnd_(0), blockStart_(0) { rowStart_(0), rowEnd_(0), blockStart_(0)
{
variableColOffsets_.push_back(0); variableColOffsets_.push_back(0);
ASSERT_INVARIANTS assertInvariants();
} }
/** Construct from a container of the sizes of each vertical block. */ /** Construct from a container of the sizes of each vertical block. */
template<typename CONTAINER> template<typename CONTAINER>
VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height) : VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) :
rowStart_(0), rowEnd_(height), blockStart_(0) { rowStart_(0), rowEnd_(height), blockStart_(0)
fillOffsets(dimensions.begin(), dimensions.end()); {
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
matrix_.resize(height, variableColOffsets_.back()); matrix_.resize(height, variableColOffsets_.back());
ASSERT_INVARIANTS 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> template<typename CONTAINER>
VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) : VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) { matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0)
fillOffsets(dimensions.begin(), dimensions.end()); {
if (variableColOffsets_.back() != matrix_.cols()) fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
throw std::invalid_argument( if(variableColOffsets_.back() != matrix_.cols())
"Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix."); throw std::invalid_argument("Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
ASSERT_INVARIANTS assertInvariants();
} }
/** /**
* Construct from iterator over the sizes of each vertical block. */ * Construct from iterator over the sizes of each vertical block. */
template<typename ITERATOR> template<typename ITERATOR>
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) :
DenseIndex height) : rowStart_(0), rowEnd_(height), blockStart_(0)
rowStart_(0), rowEnd_(height), blockStart_(0) { {
fillOffsets(firstBlockDim, lastBlockDim); fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
matrix_.resize(height, variableColOffsets_.back()); matrix_.resize(height, variableColOffsets_.back());
ASSERT_INVARIANTS assertInvariants();
} }
/** /** Copy the block structure and resize the underlying matrix, but do not copy the matrix data.
* Copy the block structure and resize the underlying matrix, but do not copy * If blockStart(), rowStart(), and/or rowEnd() have been modified, this copies the structure of
* the matrix data. If blockStart(), rowStart(), and/or rowEnd() have been * the corresponding matrix view. In the destination VerticalBlockView, blockStart() and
* modified, this copies the structure of the corresponding matrix view. In the * rowStart() will thus be 0, rowEnd() will be cols() of the source VerticalBlockView, and the
* destination VerticalBlockView, blockStart() and rowStart() will thus be 0, * underlying matrix will be the size of the view of the source matrix. */
* 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); static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs);
/** Copy the block structure, but do not copy the matrix data. If blockStart() /** Copy the block structure, but do not copy the matrix data. If blockStart() has been
* has been modified, this copies the structure of the corresponding matrix * modified, this copies the structure of the corresponding matrix view. In the destination
* view. In the destination VerticalBlockMatrix, blockStart() will be 0. */ * VerticalBlockMatrix, blockStart() will be 0. */
static VerticalBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& rhs, static VerticalBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& rhs, DenseIndex height);
DenseIndex height);
/// Row size /// Row size
inline DenseIndex rows() const { DenseIndex rows() const { assertInvariants(); return rowEnd_ - rowStart_; }
ASSERT_INVARIANTS
return rowEnd_ - rowStart_;
}
/// Column size /// Column size
inline DenseIndex cols() const { DenseIndex cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
ASSERT_INVARIANTS
return variableColOffsets_.back() - variableColOffsets_[blockStart_];
}
/// Block count /// Block count
inline DenseIndex nBlocks() const { DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
ASSERT_INVARIANTS
return variableColOffsets_.size() - 1 - blockStart_;
}
/** Access a single block in the underlying matrix with read/write access */ /** Access a single block in the underlying matrix with read/write access */
inline Block operator()(DenseIndex block) { Block operator()(DenseIndex block) { return range(block, block+1); }
return range(block, block + 1);
}
/** Access a const block view */ /** Access a const block view */
inline const constBlock operator()(DenseIndex block) const { const constBlock operator()(DenseIndex block) const { return range(block, block+1); }
return range(block, block + 1);
}
/** access ranges of blocks at a time */ /** access ranges of blocks at a time */
Block range(DenseIndex startBlock, DenseIndex endBlock) { Block range(DenseIndex startBlock, DenseIndex endBlock) {
ASSERT_INVARIANTS assertInvariants();
DenseIndex actualStartBlock = startBlock + blockStart_; DenseIndex actualStartBlock = startBlock + blockStart_;
DenseIndex actualEndBlock = endBlock + blockStart_; DenseIndex actualEndBlock = endBlock + blockStart_;
if (startBlock != 0 || endBlock != 0) { if(startBlock != 0 || endBlock != 0) {
CHECK_BLOCK(actualStartBlock); checkBlock(actualStartBlock);
assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
} }
const DenseIndex startCol = variableColOffsets_[actualStartBlock]; const DenseIndex startCol = variableColOffsets_[actualStartBlock];
@ -172,107 +137,89 @@ public:
} }
const constBlock range(DenseIndex startBlock, DenseIndex endBlock) const { const constBlock range(DenseIndex startBlock, DenseIndex endBlock) const {
ASSERT_INVARIANTS assertInvariants();
DenseIndex actualStartBlock = startBlock + blockStart_; DenseIndex actualStartBlock = startBlock + blockStart_;
DenseIndex actualEndBlock = endBlock + blockStart_; DenseIndex actualEndBlock = endBlock + blockStart_;
if (startBlock != 0 || endBlock != 0) { if(startBlock != 0 || endBlock != 0) {
CHECK_BLOCK(actualStartBlock); checkBlock(actualStartBlock);
assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
} }
const DenseIndex startCol = variableColOffsets_[actualStartBlock]; const DenseIndex startCol = variableColOffsets_[actualStartBlock];
const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol; const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
return ((const Matrix&) matrix_).block(rowStart_, startCol, this->rows(), return ((const Matrix&)matrix_).block(rowStart_, startCol, this->rows(), rangeCols);
rangeCols);
} }
/** Return the full matrix, *not* including any portions excluded by /** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */
* rowStart(), rowEnd(), and firstBlock() */ Block full() { return range(0, nBlocks()); }
inline Block full() {
return range(0, nBlocks());
}
/** Return the full matrix, *not* including any portions excluded by /** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */
* rowStart(), rowEnd(), and firstBlock() */ const constBlock full() const { return range(0, nBlocks()); }
inline const constBlock full() const {
return range(0, nBlocks());
}
inline DenseIndex offset(DenseIndex block) const { DenseIndex offset(DenseIndex block) const {
ASSERT_INVARIANTS assertInvariants();
DenseIndex actualBlock = block + blockStart_; DenseIndex actualBlock = block + blockStart_;
CHECK_BLOCK(actualBlock); checkBlock(actualBlock);
return variableColOffsets_[actualBlock]; return variableColOffsets_[actualBlock];
} }
/// Get/set the apparent first row of the underlying matrix for all operations /** Get or set the apparent first row of the underlying matrix for all operations */
inline DenseIndex& rowStart() { DenseIndex& rowStart() { return rowStart_; }
return rowStart_;
}
/** Get/set the apparent last row /** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
* (exclusive, i.e. rows() == rowEnd() - rowStart()) DenseIndex& rowEnd() { return rowEnd_; }
* of the underlying matrix for all operations */
inline DenseIndex& rowEnd() {
return rowEnd_;
}
/** Get/set the apparent first block for all operations */ /** Get or set the apparent first block for all operations */
inline DenseIndex& firstBlock() { DenseIndex& firstBlock() { return blockStart_; }
return blockStart_;
}
/** Get the apparent first row of the underlying matrix for all operations */ /** Get the apparent first row of the underlying matrix for all operations */
inline DenseIndex rowStart() const { DenseIndex rowStart() const { return rowStart_; }
return rowStart_;
}
/** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) /** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
* of the underlying matrix for all operations */ DenseIndex rowEnd() const { return rowEnd_; }
inline DenseIndex rowEnd() const {
return rowEnd_;
}
/** Get the apparent first block for all operations */ /** Get the apparent first block for all operations */
inline DenseIndex firstBlock() const { DenseIndex firstBlock() const { return blockStart_; }
return blockStart_;
}
/** Access to full matrix (*including* any portions excluded by rowStart(), /** Access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */
* rowEnd(), and firstBlock()) */ const Matrix& matrix() const { return matrix_; }
inline const Matrix& matrix() const {
return matrix_;
}
/** Non-const access to full matrix (*including* any portions excluded by /** Non-const access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */
* rowStart(), rowEnd(), and firstBlock()) */ Matrix& matrix() { return matrix_; }
inline Matrix& matrix() {
return matrix_;
}
protected:
protected:
void assertInvariants() const { void assertInvariants() const {
ASSERT_INVARIANTS assert(matrix_.cols() == variableColOffsets_.back());
assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
assert(rowStart_ <= matrix_.rows());
assert(rowEnd_ <= matrix_.rows());
assert(rowStart_ <= rowEnd_);
} }
void checkBlock(DenseIndex block) const { void checkBlock(DenseIndex block) const {
CHECK_BLOCK(block) assert(matrix_.cols() == variableColOffsets_.back());
assert(block < (DenseIndex)variableColOffsets_.size() - 1);
assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
} }
template<typename ITERATOR> template<typename ITERATOR>
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) { void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) {
variableColOffsets_.resize((lastBlockDim - firstBlockDim) + 1); variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
variableColOffsets_[0] = 0; variableColOffsets_[0] = 0;
DenseIndex j = 0; DenseIndex j=0;
for (ITERATOR dim = firstBlockDim; dim != lastBlockDim; ++dim) { for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
variableColOffsets_[j + 1] = variableColOffsets_[j] + *dim; variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
++j; ++ j;
}
if(appendOneDimension)
{
variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
++ j;
} }
} }
friend class SymmetricBlockMatrix; friend class SymmetricBlockMatrix;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
@ -283,6 +230,6 @@ private:
ar & BOOST_SERIALIZATION_NVP(rowEnd_); ar & BOOST_SERIALIZATION_NVP(rowEnd_);
ar & BOOST_SERIALIZATION_NVP(blockStart_); ar & BOOST_SERIALIZATION_NVP(blockStart_);
} }
}; };
} }

3
gtsam/linear/HessianFactor.cpp
View File

@ -42,7 +42,6 @@
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/transformed.hpp> #include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp> #include <boost/range/adaptor/map.hpp>
#include <boost/range/join.hpp>
#include <boost/range/algorithm/copy.hpp> #include <boost/range/algorithm/copy.hpp>
#include <sstream> #include <sstream>
@ -182,7 +181,7 @@ DenseIndex _getSizeHF(const Vector& m) { return m.size(); }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs, HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs, double f) : const std::vector<Vector>& gs, double f) :
GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), ListOfOne((DenseIndex)1))) GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true)
{ {
// Get the number of variables // Get the number of variables
size_t variable_count = js.size(); size_t variable_count = js.size();

3
gtsam/linear/JacobianFactor-inl.h
View File

@ -21,7 +21,6 @@
#include <gtsam/linear/linearExceptions.h> #include <gtsam/linear/linearExceptions.h>
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/transformed.hpp> #include <boost/range/adaptor/transformed.hpp>
#include <boost/range/join.hpp>
#include <boost/range/algorithm/for_each.hpp> #include <boost/range/algorithm/for_each.hpp>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
@ -123,7 +122,7 @@ namespace gtsam {
// matrices, then extract the number of columns e.g. dimensions in each matrix. Then joins with // 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. // a single '1' to add a dimension for the b vector.
{ {
Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&internal::getColsJF), ListOfOne((DenseIndex)1)), b.size()); Ab_ = VerticalBlockMatrix(terms | transformed(&internal::getColsJF), b.size(), true);
} }
// Check and add terms // Check and add terms

2
gtsam/linear/JacobianFactor.cpp
View File

@ -283,7 +283,7 @@ namespace gtsam {
// Allocate matrix and copy keys in order // Allocate matrix and copy keys in order
gttic(allocate); gttic(allocate);
Ab_ = VerticalBlockMatrix(boost::join(varDims, ListOfOne((DenseIndex)1)), m); // Allocate augmented matrix Ab_ = VerticalBlockMatrix(varDims, m, true); // Allocate augmented matrix
Base::keys_.resize(orderedSlots.size()); Base::keys_.resize(orderedSlots.size());
boost::range::copy( // Get variable keys boost::range::copy( // Get variable keys
orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin()); orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin());