diff --git a/.cproject b/.cproject index 88abf2f1e..e6e040b97 100644 --- a/.cproject +++ b/.cproject @@ -2092,6 +2092,22 @@ true true + + make + -j5 + testSymmetricBlockMatrix.run + true + true + true + + + make + -j5 + testVerticalBlockMatrix.run + true + true + true + make -j5 diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index 98fe1be5d..f1ed8972e 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -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.cpp -* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional. -* @author Richard Roberts -* @date Sep 18, 2010 -*/ + * @file SymmetricBlockMatrix.cpp + * @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional. + * @author Richard Roberts + * @date Sep 18, 2010 + */ #include #include @@ -23,56 +23,60 @@ namespace gtsam { - /* ************************************************************************* */ - SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(const SymmetricBlockMatrix& other) - { - SymmetricBlockMatrix result; - result.variableColOffsets_.resize(other.nBlocks() + 1); - for(size_t i = 0; i < result.variableColOffsets_.size(); ++i) - result.variableColOffsets_[i] = - other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_]; - result.matrix_.resize(other.cols(), other.cols()); - result.assertInvariants(); - return result; - } - - /* ************************************************************************* */ - SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& other) - { - SymmetricBlockMatrix result; - result.variableColOffsets_.resize(other.nBlocks() + 1); - for(size_t i = 0; i < result.variableColOffsets_.size(); ++i) - result.variableColOffsets_[i] = - other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_]; - result.matrix_.resize(other.cols(), other.cols()); - result.assertInvariants(); - return result; - } - - /* ************************************************************************* */ - VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial(DenseIndex nFrontals) - { - // Do dense elimination - if(!blockStart() == 0) - throw std::invalid_argument("Can only do Cholesky when the SymmetricBlockMatrix is not a restricted view, i.e. when blockStart == 0."); - if(!gtsam::choleskyPartial(matrix_, offset(nFrontals))) - throw CholeskyFailed(); - - // Split conditional - - // Create one big conditionals with many frontal variables. - gttic(Construct_eliminated); - const size_t varDim = offset(nFrontals); - VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim); - Ab.full() = matrix_.topRows(varDim); - Ab.full().triangularView().setZero(); - gttoc(Construct_conditional); - - gttic(Remaining_factor); - // Take lower-right block of Ab_ to get the remaining factor - blockStart() = nFrontals; - gttoc(Remaining_factor); - - return Ab; - } +/* ************************************************************************* */ +SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf( + const SymmetricBlockMatrix& other) { + SymmetricBlockMatrix result; + result.variableColOffsets_.resize(other.nBlocks() + 1); + for (size_t i = 0; i < result.variableColOffsets_.size(); ++i) + result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_ + + i] - other.variableColOffsets_[other.blockStart_]; + result.matrix_.resize(other.cols(), other.cols()); + result.assertInvariants(); + return result; } + +/* ************************************************************************* */ +SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf( + const VerticalBlockMatrix& other) { + SymmetricBlockMatrix result; + result.variableColOffsets_.resize(other.nBlocks() + 1); + for (size_t i = 0; i < result.variableColOffsets_.size(); ++i) + result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_ + + i] - other.variableColOffsets_[other.blockStart_]; + result.matrix_.resize(other.cols(), other.cols()); + result.assertInvariants(); + return result; +} + +/* ************************************************************************* */ +VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial( + DenseIndex nFrontals) { + // Do dense elimination + if (!blockStart() == 0) + throw std::invalid_argument( + "Can only do Cholesky when the SymmetricBlockMatrix is not a restricted view, i.e. when blockStart == 0."); + if (!gtsam::choleskyPartial(matrix_, offset(nFrontals))) + throw CholeskyFailed(); + + // Split conditional + + // Create one big conditionals with many frontal variables. + gttic(Construct_eliminated); + const size_t varDim = offset(nFrontals); + VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim); + Ab.full() = matrix_.topRows(varDim); + Ab.full().triangularView().setZero(); + gttoc(Construct_conditional); + + gttic(Remaining_factor); + // Take lower-right block of Ab_ to get the remaining factor + blockStart() = nFrontals; + gttoc(Remaining_factor); + + return Ab; +} +/* ************************************************************************* */ + +} //\ namespace gtsam + diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 14014898a..de62bd8d7 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,296 +23,352 @@ namespace gtsam { - // Forward declarations - class VerticalBlockMatrix; +// 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(); + } /** - * 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; + * @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(); + } - 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 SymmetricBlockMatrix& other); - 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. + * In the destination SymmetricBlockMatrix, blockStart() will be 0. + */ + static SymmetricBlockMatrix LikeActiveViewOf( + const VerticalBlockMatrix& other); - public: - /// Construct from an empty matrix (asserts that the matrix is empty) - SymmetricBlockMatrix() : - blockStart_(0) - { - variableColOffsets_.push_back(0); - assertInvariants(); + /// 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; } + } - /// 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(); - } + friend class VerticalBlockMatrix; + template friend class SymmetricBlockMatrixBlockExpr; - /// 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(); - } +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 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(); - } - - /// 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); +/* ************************************************************************* */ +class CholeskyFailed: public gtsam::ThreadsafeException { +public: + CholeskyFailed() throw () { + } + virtual ~CholeskyFailed() throw () { + } +}; - /// 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; - } - } - - 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() {} - }; - -} +} //\ namespace gtsam diff --git a/gtsam/base/VerticalBlockMatrix.cpp b/gtsam/base/VerticalBlockMatrix.cpp index 0361264fe..9f54b9c97 100644 --- a/gtsam/base/VerticalBlockMatrix.cpp +++ b/gtsam/base/VerticalBlockMatrix.cpp @@ -21,32 +21,32 @@ namespace gtsam { - /* ************************************************************************* */ - VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& other) - { - VerticalBlockMatrix result; - result.variableColOffsets_.resize(other.nBlocks() + 1); - for(size_t i = 0; i < result.variableColOffsets_.size(); ++i) - result.variableColOffsets_[i] = - other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_]; - result.matrix_.resize(other.rows(), result.variableColOffsets_.back()); - result.rowEnd_ = other.rows(); - result.assertInvariants(); - return result; - } +/* ************************************************************************* */ +VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf( + const VerticalBlockMatrix& other) { + VerticalBlockMatrix result; + result.variableColOffsets_.resize(other.nBlocks() + 1); + for (size_t i = 0; i < result.variableColOffsets_.size(); ++i) + result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_ + + i] - other.variableColOffsets_[other.blockStart_]; + result.matrix_.resize(other.rows(), result.variableColOffsets_.back()); + result.rowEnd_ = other.rows(); + result.assertInvariants(); + return result; +} - /* ************************************************************************* */ - VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const SymmetricBlockMatrix& other, DenseIndex height) - { - VerticalBlockMatrix result; - result.variableColOffsets_.resize(other.nBlocks() + 1); - for(size_t i = 0; i < result.variableColOffsets_.size(); ++i) - result.variableColOffsets_[i] = - other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_]; - result.matrix_.resize(height, result.variableColOffsets_.back()); - result.rowEnd_ = height; - result.assertInvariants(); - return result; - } +/* ************************************************************************* */ +VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf( + const SymmetricBlockMatrix& other, DenseIndex height) { + VerticalBlockMatrix result; + result.variableColOffsets_.resize(other.nBlocks() + 1); + for (size_t i = 0; i < result.variableColOffsets_.size(); ++i) + result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_ + + i] - other.variableColOffsets_[other.blockStart_]; + result.matrix_.resize(height, result.variableColOffsets_.back()); + result.rowEnd_ = height; + result.assertInvariants(); + return result; +} } diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 0e45a8f0d..f26f44d91 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -22,209 +22,261 @@ 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 + + /// 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. + +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(); + } /** - * 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 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(); + } - protected: - Matrix matrix_; ///< The full matrix - FastVector variableColOffsets_; ///< the starting columns of each block (0-based) + /** + * 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(); + } - 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 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); - public: + /** 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); - /** Construct an empty VerticalBlockMatrix */ - VerticalBlockMatrix() : - rowStart_(0), rowEnd_(0), blockStart_(0) - { - variableColOffsets_.push_back(0); - assertInvariants(); + /// 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); + } - /** 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/set the apparent first row of the underlying matrix for all operations + DenseIndex& rowStart() { + return rowStart_; + } + + /** Get/set the apparent last row + * (exclusive, i.e. rows() == rowEnd() - rowStart()) + * of the underlying matrix for all operations */ + DenseIndex& rowEnd() { + return rowEnd_; + } + + /** Get/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_); + } +}; } diff --git a/gtsam/base/tests/testVerticalBlockMatrix.cpp b/gtsam/base/tests/testVerticalBlockMatrix.cpp new file mode 100644 index 000000000..fad23fa7d --- /dev/null +++ b/gtsam/base/tests/testVerticalBlockMatrix.cpp @@ -0,0 +1,47 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testVerticalBlockMatrix.cpp + * @brief Unit tests for VerticalBlockMatrix class + * @author Frank Dellaert + * @date February 15, 2014 + **/ + +#include +#include +#include + +using namespace std; +using namespace gtsam; +using boost::assign::list_of; + +//***************************************************************************** +TEST(VerticalBlockMatrix, constructor) { + VerticalBlockMatrix actual(list_of(3)(2)(1), + (Matrix(6, 6) << 1, 2, 3, 4, 5, 6, // + 2, 8, 9, 10, 11, 12, // + 3, 9, 15, 16, 17, 18, // + 4, 10, 16, 22, 23, 24, // + 5, 11, 17, 23, 29, 30, // + 6, 12, 18, 24, 30, 36)); + EXPECT_LONGS_EQUAL(6,actual.rows()); + EXPECT_LONGS_EQUAL(6,actual.cols()); + EXPECT_LONGS_EQUAL(3,actual.nBlocks()); +} + +//***************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//***************************************************************************** +