From 99947c7c6daf1c1ee79a8ffb91b8901b816582e4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 15 Feb 2014 11:55:00 -0500 Subject: [PATCH] Format using BORG conventions --- gtsam/base/VerticalBlockMatrix.h | 415 +++++++++++++++++-------------- 1 file changed, 224 insertions(+), 191 deletions(-) diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 0e45a8f0d..62e5377b2 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -22,209 +22,242 @@ namespace gtsam { - // Forward declarations - class SymmetricBlockMatrix; +// 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 + FastVector variableColOffsets_; ///< the starting columns of each block (0-based) + + 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. + +public: + + /** Construct an empty VerticalBlockMatrix */ + VerticalBlockMatrix() : + rowStart_(0), rowEnd_(0), blockStart_(0) { + variableColOffsets_.push_back(0); + assertInvariants(); + } + + /** 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()); + assertInvariants(); + } + + /** 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."); + assertInvariants(); + } /** - * 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()); + assertInvariants(); + } - 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 + DenseIndex rows() const { + assertInvariants(); + return rowEnd_ - rowStart_; + } - /** Construct an empty VerticalBlockMatrix */ - VerticalBlockMatrix() : - rowStart_(0), rowEnd_(0), blockStart_(0) - { - variableColOffsets_.push_back(0); - assertInvariants(); + /// 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); + } - /** 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()); - assertInvariants(); + 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); + } - /** 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."); - assertInvariants(); + /** 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) { + 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 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()); - 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); + friend class SymmetricBlockMatrix; - /** 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) { - 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; - } - } - - 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_); - } - }; +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_); + } +}; }