diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index de62bd8d7..45edbf6af 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -1,20 +1,20 @@ /* ---------------------------------------------------------------------------- - * 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) +* 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 +* See LICENSE for the license information - * -------------------------------------------------------------------------- */ +* -------------------------------------------------------------------------- */ /** - * @file SymmetricBlockMatrix.h - * @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional. - * @author Richard Roberts - * @date Sep 18, 2010 - */ +* @file SymmetricBlockMatrix.h +* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional. +* @author Richard Roberts +* @date Sep 18, 2010 +*/ #pragma once #include @@ -23,352 +23,232 @@ namespace gtsam { -// Forward declarations -class VerticalBlockMatrix; - -/** - * This class stores a dense matrix and allows it to be accessed as a - * collection of blocks. When constructed, the caller must provide the - * dimensions of the blocks. - * - * The block structure is symmetric, but the underlying matrix does not - * necessarily need to be. - * - * This class also has a parameter that can be changed after construction to - * change the apparent matrix view. firstBlock() determines the block that - * appears to have index 0 for all operations (except re-setting firstBlock). - * - * @addtogroup base */ -class GTSAM_EXPORT SymmetricBlockMatrix { -public: - typedef SymmetricBlockMatrix This; - typedef SymmetricBlockMatrixBlockExpr Block; - typedef SymmetricBlockMatrixBlockExpr constBlock; - -protected: - - Matrix matrix_; ///< The full matrix - - /// the starting columns of each block (0-based) - FastVector variableColOffsets_; - - /// Changes apparent matrix view, see main class comment. - DenseIndex blockStart_; - -public: - /// Construct from an empty matrix (asserts that the matrix is empty) - SymmetricBlockMatrix() : - blockStart_(0) { - variableColOffsets_.push_back(0); - assertInvariants(); - } - - /// Construct from a container of the sizes of each block. - template - SymmetricBlockMatrix(const CONTAINER& dimensions) : - blockStart_(0) { - fillOffsets(dimensions.begin(), dimensions.end()); - matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); - assertInvariants(); - } - - /// Construct from iterator over the sizes of each vertical block. - template - SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim) : - blockStart_(0) { - fillOffsets(firstBlockDim, lastBlockDim); - matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); - assertInvariants(); - } + // Forward declarations + class VerticalBlockMatrix; /** - * @brief Construct from a container of the sizes of each vertical block - * and a pre-prepared matrix. - */ - template - SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) : - blockStart_(0) { - matrix_.resize(matrix.rows(), matrix.cols()); - matrix_.triangularView() = - matrix.triangularView(); - fillOffsets(dimensions.begin(), dimensions.end()); - if (matrix_.rows() != matrix_.cols()) - throw std::invalid_argument("Requested to create a SymmetricBlockMatrix" - " from a non-square 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(); - } + * This class stores a dense matrix and allows it to be accessed as a collection of blocks. When + * constructed, the caller must provide the dimensions of the blocks. + * + * The block structure is symmetric, but the underlying matrix does not necessarily need to be. + * + * This class also has a parameter that can be changed after construction to change the apparent + * matrix view. firstBlock() determines the block that appears to have index 0 for all operations + * (except re-setting firstBlock()). + * + * @addtogroup base */ + class GTSAM_EXPORT SymmetricBlockMatrix + { + public: + typedef SymmetricBlockMatrix This; + typedef SymmetricBlockMatrixBlockExpr Block; + typedef SymmetricBlockMatrixBlockExpr constBlock; - /** - * Copy the block structure, but do not copy the matrix data. If blockStart() - * has been modified, this copies the structure of the corresponding matrix. - * In the destination SymmetricBlockMatrix, blockStart() will be 0. - */ - static SymmetricBlockMatrix LikeActiveViewOf( - const SymmetricBlockMatrix& other); + protected: + Matrix matrix_; ///< The full matrix + FastVector variableColOffsets_; ///< the starting columns of each block (0-based) - /** - * Copy the block structure, but do not copy the matrix data. If blockStart() - * has been modified, this copies the structure of the corresponding matrix. - * In the destination SymmetricBlockMatrix, blockStart() will be 0. - */ - static SymmetricBlockMatrix LikeActiveViewOf( - const VerticalBlockMatrix& other); + DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment. - /// Row size - DenseIndex rows() const { - assertInvariants(); - return variableColOffsets_.back() - variableColOffsets_[blockStart_]; - } - - /// Column size - DenseIndex cols() const { - return rows(); - } - - /// 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 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 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(); - 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(); - 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 Block(*this, 0, nBlocks(), 0); - } - - /** - * Return the full matrix, *not* including any portions excluded by - * firstBlock(). - */ - constBlock full() const { - return constBlock(*this, 0, nBlocks(), 0); - } - - /** - * Access to full matrix, including any portions excluded by firstBlock() - * to other operations. - */ - Eigen::SelfAdjointView matrix() const { - return matrix_; - } - - /** Access to full matrix, including any portions excluded by firstBlock() - * to other operations. - */ - Eigen::SelfAdjointView matrix() { - return matrix_; - } - - /** - * 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 { - assert(matrix_.rows() == matrix_.cols()); - assert(matrix_.cols() == variableColOffsets_.back()); - assert(blockStart_ < (DenseIndex)variableColOffsets_.size()); - } - - 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)->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)->rangeUnchecked(startBlock, endBlock); - // return constBlock(matrix(), block.startRow(), block.startCol(), block.rows(), block.cols()); - //} - - template - void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) { - variableColOffsets_.resize((lastBlockDim - firstBlockDim) + 1); - variableColOffsets_[0] = 0; - DenseIndex j = 0; - for (ITERATOR dim = firstBlockDim; dim != lastBlockDim; ++dim) { - variableColOffsets_[j + 1] = variableColOffsets_[j] + *dim; - ++j; + public: + /// Construct from an empty matrix (asserts that the matrix is empty) + SymmetricBlockMatrix() : + blockStart_(0) + { + variableColOffsets_.push_back(0); + assertInvariants(); } - } - friend class VerticalBlockMatrix; - template friend class SymmetricBlockMatrixBlockExpr; + /// Construct from a container of the sizes of each block. + template + SymmetricBlockMatrix(const CONTAINER& dimensions, bool appendOneDimension = false) : + blockStart_(0) + { + fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); + matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); + assertInvariants(); + } -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(matrix_); - ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); - ar & BOOST_SERIALIZATION_NVP(blockStart_); - } -}; + /// Construct from iterator over the sizes of each vertical block. + template + SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension = false) : + blockStart_(0) + { + fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); + matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); + assertInvariants(); + } -/* ************************************************************************* */ -class CholeskyFailed: public gtsam::ThreadsafeException { -public: - CholeskyFailed() throw () { - } - virtual ~CholeskyFailed() throw () { - } -}; + /// Construct from a container of the sizes of each vertical block and a pre-prepared matrix. + template + SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) : + blockStart_(0) + { + matrix_.resize(matrix.rows(), matrix.cols()); + matrix_.triangularView() = matrix.triangularView(); + fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); + if(matrix_.rows() != matrix_.cols()) + throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square 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(); + } + + /// 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. + static SymmetricBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& other); -} //\ namespace gtsam + /// Row size + DenseIndex rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } + + /// Column size + DenseIndex cols() const { return rows(); } + + /// 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 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 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(); + 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(); + 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 Block(*this, 0, nBlocks(), 0); + } + + /** Return the full matrix, *not* including any portions excluded by firstBlock(). */ + constBlock full() const + { + return constBlock(*this, 0, nBlocks(), 0); + } + + /** Access to full matrix, including any portions excluded by firstBlock() to other operations. */ + Eigen::SelfAdjointView matrix() const + { + return matrix_; + } + + /** Access to full matrix, including any portions excluded by firstBlock() to other operations. */ + Eigen::SelfAdjointView matrix() + { + return matrix_; + } + + /// 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 + { + assert(matrix_.rows() == matrix_.cols()); + assert(matrix_.cols() == variableColOffsets_.back()); + assert(blockStart_ < (DenseIndex)variableColOffsets_.size()); + } + + 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_]; + } + + template + void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) + { + variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0)); + variableColOffsets_[0] = 0; + DenseIndex j=0; + for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { + variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; + ++ j; + } + if(appendOneDimension) + { + variableColOffsets_[j+1] = variableColOffsets_[j] + 1; + ++ j; + } + } + + friend class VerticalBlockMatrix; + template friend class SymmetricBlockMatrixBlockExpr; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); + ar & BOOST_SERIALIZATION_NVP(blockStart_); + } + }; + + /* ************************************************************************* */ + class CholeskyFailed : public gtsam::ThreadsafeException + { + public: + CholeskyFailed() throw() {} + virtual ~CholeskyFailed() throw() {} + }; + +} diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index fd05545b9..029f55c58 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -22,267 +22,214 @@ namespace gtsam { -// Forward declarations -class SymmetricBlockMatrix; - -/** - * This class stores a dense matrix and allows it to be accessed as a collection - * of vertical blocks. - * - * 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). - * - * @addtogroup base - */ -class GTSAM_EXPORT VerticalBlockMatrix { -public: - typedef VerticalBlockMatrix This; - typedef Eigen::Block Block; - typedef Eigen::Block constBlock; - -protected: - Matrix matrix_; ///< The full matrix - - /// the starting columns of each block (0-based) - FastVector variableColOffsets_; - - DenseIndex rowStart_; ///< Changes apparent matrix view, see class comments. - 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 */ - VerticalBlockMatrix() : - rowStart_(0), rowEnd_(0), blockStart_(0) { - variableColOffsets_.push_back(0); - ASSERT_INVARIANTS - } - - /** Construct from a container of the sizes of each vertical block. */ - template - VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height) : - rowStart_(0), rowEnd_(height), blockStart_(0) { - fillOffsets(dimensions.begin(), dimensions.end()); - matrix_.resize(height, variableColOffsets_.back()); - ASSERT_INVARIANTS - } + // Forward declarations + class SymmetricBlockMatrix; /** - * Construct from a container of the sizes of each vertical block and a - * pre-prepared matrix. - */ - template - VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) : - matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) { - fillOffsets(dimensions.begin(), dimensions.end()); - if (variableColOffsets_.back() != matrix_.cols()) - 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 - } + * This class stores a dense matrix and allows it to be accessed as a collection of vertical + * blocks. 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). + * + * @addtogroup base */ + class GTSAM_EXPORT VerticalBlockMatrix + { + public: + typedef VerticalBlockMatrix This; + typedef Eigen::Block Block; + typedef Eigen::Block constBlock; - /** - * Construct from iterator over the sizes of each vertical block. */ - template - VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, - DenseIndex height) : - rowStart_(0), rowEnd_(height), blockStart_(0) { - fillOffsets(firstBlockDim, lastBlockDim); - matrix_.resize(height, variableColOffsets_.back()); - ASSERT_INVARIANTS - } + protected: + Matrix matrix_; ///< The full matrix + FastVector variableColOffsets_; ///< the starting columns of each block (0-based) - /** - * Copy the block structure and resize the underlying matrix, but do not copy - * the matrix data. If blockStart(), rowStart(), and/or rowEnd() have been - * modified, this copies the structure of the corresponding matrix view. In the - * destination VerticalBlockView, blockStart() and rowStart() will thus be 0, - * rowEnd() will be cols() of the source VerticalBlockView, and the - * underlying matrix will be the size of the view of the source matrix. - */ - static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs); + DenseIndex rowStart_; ///< Changes apparent matrix view, see main class comment. + DenseIndex rowEnd_; ///< Changes apparent matrix view, see main class comment. + DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment. - /** 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 VerticalBlockMatrix, blockStart() will be 0. */ - static VerticalBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& rhs, - DenseIndex height); + public: - /// Row size - inline DenseIndex rows() const { - ASSERT_INVARIANTS - return rowEnd_ - rowStart_; - } - - /// Column size - inline DenseIndex cols() const { - ASSERT_INVARIANTS - return variableColOffsets_.back() - variableColOffsets_[blockStart_]; - } - - /// Block count - inline DenseIndex nBlocks() const { - ASSERT_INVARIANTS - return variableColOffsets_.size() - 1 - blockStart_; - } - - /** Access a single block in the underlying matrix with read/write access */ - inline Block operator()(DenseIndex block) { - return range(block, block + 1); - } - - /** Access a const block view */ - inline const constBlock operator()(DenseIndex block) const { - return range(block, block + 1); - } - - /** access ranges of blocks at a time */ - Block range(DenseIndex startBlock, DenseIndex endBlock) { - ASSERT_INVARIANTS - DenseIndex actualStartBlock = startBlock + blockStart_; - DenseIndex actualEndBlock = endBlock + blockStart_; - if (startBlock != 0 || endBlock != 0) { - CHECK_BLOCK(actualStartBlock); - assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); + /** Construct an empty VerticalBlockMatrix */ + VerticalBlockMatrix() : + rowStart_(0), rowEnd_(0), blockStart_(0) + { + variableColOffsets_.push_back(0); + assertInvariants(); } - const DenseIndex startCol = variableColOffsets_[actualStartBlock]; - const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol; - return matrix_.block(rowStart_, startCol, this->rows(), rangeCols); - } - const constBlock range(DenseIndex startBlock, DenseIndex endBlock) const { - ASSERT_INVARIANTS - DenseIndex actualStartBlock = startBlock + blockStart_; - DenseIndex actualEndBlock = endBlock + blockStart_; - if (startBlock != 0 || endBlock != 0) { - CHECK_BLOCK(actualStartBlock); - assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); + /** Construct from a container of the sizes of each vertical block. */ + template + VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) : + rowStart_(0), rowEnd_(height), blockStart_(0) + { + fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); + matrix_.resize(height, variableColOffsets_.back()); + assertInvariants(); } - const DenseIndex startCol = variableColOffsets_[actualStartBlock]; - const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol; - return ((const Matrix&) matrix_).block(rowStart_, startCol, this->rows(), - rangeCols); - } - /** Return the full matrix, *not* including any portions excluded by - * rowStart(), rowEnd(), and firstBlock() */ - inline Block full() { - return range(0, nBlocks()); - } - - /** Return the full matrix, *not* including any portions excluded by - * rowStart(), rowEnd(), and firstBlock() */ - inline const constBlock full() const { - return range(0, nBlocks()); - } - - inline DenseIndex offset(DenseIndex block) const { - ASSERT_INVARIANTS - DenseIndex actualBlock = block + blockStart_; - CHECK_BLOCK(actualBlock); - return variableColOffsets_[actualBlock]; - } - - /// Get/set the apparent first row of the underlying matrix for all operations - inline DenseIndex& rowStart() { - return rowStart_; - } - - /** Get/set the apparent last row - * (exclusive, i.e. rows() == rowEnd() - rowStart()) - * of the underlying matrix for all operations */ - inline DenseIndex& rowEnd() { - return rowEnd_; - } - - /** Get/set the apparent first block for all operations */ - inline DenseIndex& firstBlock() { - return blockStart_; - } - - /** Get the apparent first row of the underlying matrix for all operations */ - inline DenseIndex rowStart() const { - return rowStart_; - } - - /** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) - * of the underlying matrix for all operations */ - inline DenseIndex rowEnd() const { - return rowEnd_; - } - - /** Get the apparent first block for all operations */ - inline DenseIndex firstBlock() const { - return blockStart_; - } - - /** Access to full matrix (*including* any portions excluded by rowStart(), - * rowEnd(), and firstBlock()) */ - inline const Matrix& matrix() const { - return matrix_; - } - - /** Non-const access to full matrix (*including* any portions excluded by - * rowStart(), rowEnd(), and firstBlock()) */ - inline Matrix& matrix() { - return matrix_; - } - -protected: - - void assertInvariants() const { - ASSERT_INVARIANTS - } - - void checkBlock(DenseIndex block) const { - CHECK_BLOCK(block) - } - - template - void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) { - variableColOffsets_.resize((lastBlockDim - firstBlockDim) + 1); - variableColOffsets_[0] = 0; - DenseIndex j = 0; - for (ITERATOR dim = firstBlockDim; dim != lastBlockDim; ++dim) { - variableColOffsets_[j + 1] = variableColOffsets_[j] + *dim; - ++j; + /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ + template + VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) : + matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) + { + fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); + if(variableColOffsets_.back() != matrix_.cols()) + throw std::invalid_argument("Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix."); + assertInvariants(); } - } - friend class SymmetricBlockMatrix; + /** + * Construct from iterator over the sizes of each vertical block. */ + template + VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) : + rowStart_(0), rowEnd_(height), blockStart_(0) + { + fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); + matrix_.resize(height, variableColOffsets_.back()); + assertInvariants(); + } + + /** Copy the block structure and resize the underlying matrix, but do not copy the matrix data. + * If blockStart(), rowStart(), and/or rowEnd() have been modified, this copies the structure of + * the corresponding matrix view. In the destination VerticalBlockView, blockStart() and + * rowStart() will thus be 0, rowEnd() will be cols() of the source VerticalBlockView, and the + * underlying matrix will be the size of the view of the source matrix. */ + static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs); -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(matrix_); - ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); - ar & BOOST_SERIALIZATION_NVP(rowStart_); - ar & BOOST_SERIALIZATION_NVP(rowEnd_); - ar & BOOST_SERIALIZATION_NVP(blockStart_); - } -}; + /** 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 + * VerticalBlockMatrix, blockStart() will be 0. */ + static VerticalBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& rhs, DenseIndex height); + + /// Row size + DenseIndex rows() const { assertInvariants(); return rowEnd_ - rowStart_; } + + /// Column size + DenseIndex cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } + + /// Block count + DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } + + /** Access a single block in the underlying matrix with read/write access */ + Block operator()(DenseIndex block) { return range(block, block+1); } + + /** Access a const block view */ + const constBlock operator()(DenseIndex block) const { return range(block, block+1); } + + /** access ranges of blocks at a time */ + Block range(DenseIndex startBlock, DenseIndex endBlock) { + assertInvariants(); + DenseIndex actualStartBlock = startBlock + blockStart_; + DenseIndex actualEndBlock = endBlock + blockStart_; + if(startBlock != 0 || endBlock != 0) { + checkBlock(actualStartBlock); + assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); + } + const DenseIndex startCol = variableColOffsets_[actualStartBlock]; + const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol; + return matrix_.block(rowStart_, startCol, this->rows(), rangeCols); + } + + const constBlock range(DenseIndex startBlock, DenseIndex endBlock) const { + assertInvariants(); + DenseIndex actualStartBlock = startBlock + blockStart_; + DenseIndex actualEndBlock = endBlock + blockStart_; + if(startBlock != 0 || endBlock != 0) { + checkBlock(actualStartBlock); + assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); + } + const DenseIndex startCol = variableColOffsets_[actualStartBlock]; + const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol; + return ((const Matrix&)matrix_).block(rowStart_, startCol, this->rows(), rangeCols); + } + + /** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */ + Block full() { return range(0, nBlocks()); } + + /** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */ + const constBlock full() const { return range(0, nBlocks()); } + + DenseIndex offset(DenseIndex block) const { + assertInvariants(); + DenseIndex actualBlock = block + blockStart_; + checkBlock(actualBlock); + return variableColOffsets_[actualBlock]; + } + + /** Get or set the apparent first row of the underlying matrix for all operations */ + DenseIndex& rowStart() { return rowStart_; } + + /** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ + DenseIndex& rowEnd() { return rowEnd_; } + + /** Get or set the apparent first block for all operations */ + DenseIndex& firstBlock() { return blockStart_; } + + /** Get the apparent first row of the underlying matrix for all operations */ + DenseIndex rowStart() const { return rowStart_; } + + /** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ + DenseIndex rowEnd() const { return rowEnd_; } + + /** Get the apparent first block for all operations */ + DenseIndex firstBlock() const { return blockStart_; } + + /** Access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */ + const Matrix& matrix() const { return matrix_; } + + /** Non-const access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */ + Matrix& matrix() { return matrix_; } + + protected: + void assertInvariants() const { + 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 { + assert(matrix_.cols() == variableColOffsets_.back()); + assert(block < (DenseIndex)variableColOffsets_.size() - 1); + assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols()); + } + + template + void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) { + variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0)); + variableColOffsets_[0] = 0; + DenseIndex j=0; + for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { + variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; + ++ j; + } + if(appendOneDimension) + { + variableColOffsets_[j+1] = variableColOffsets_[j] + 1; + ++ j; + } + } + + friend class SymmetricBlockMatrix; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); + ar & BOOST_SERIALIZATION_NVP(rowStart_); + ar & BOOST_SERIALIZATION_NVP(rowEnd_); + ar & BOOST_SERIALIZATION_NVP(blockStart_); + } + }; } diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index ce5f093dc..fced268ca 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -42,7 +42,6 @@ #include #include #include -#include #include #include @@ -182,7 +181,7 @@ DenseIndex _getSizeHF(const Vector& m) { return m.size(); } /* ************************************************************************* */ HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, const std::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 size_t variable_count = js.size(); diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index f391d4afd..b2946b1fc 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -21,7 +21,6 @@ #include #include #include -#include #include #include @@ -123,7 +122,7 @@ namespace gtsam { // 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. { - 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 diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 809f54b99..a374a37d4 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -283,7 +283,7 @@ namespace gtsam { // Allocate matrix and copy keys in order 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()); boost::range::copy( // Get variable keys orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin());