diff --git a/gtsam/base/blockMatrices.h b/gtsam/base/blockMatrices.h deleted file mode 100644 index 973d63247..000000000 --- a/gtsam/base/blockMatrices.h +++ /dev/null @@ -1,627 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 blockMatrices.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 - -namespace gtsam { - -template class SymmetricBlockView; - -/** - * This class stores a *reference* to a matrix and allows it to be accessed as - * a collection of vertical blocks. It also provides for accessing individual - * columns from those blocks. When constructed or resized, the caller must - * provide the dimensions of the blocks, as well as an underlying matrix - * storage object. This class will resize the underlying matrix such that it - * is consistent with the given block dimensions. - * - * This class also has three parameters that can be changed after construction - * that change the apparent view of the matrix. 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* 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 - */ -template -class VerticalBlockView { -public: - typedef MATRIX FullMatrix; - typedef Eigen::Block Block; - typedef Eigen::Block constBlock; - - // columns of blocks - typedef Eigen::VectorBlock Column; - typedef Eigen::VectorBlock constColumn; - -protected: - FullMatrix& matrix_; // the reference to the full matrix - std::vector variableColOffsets_; // the starting columns of each block (0-based) - - // Changes apparent matrix view, see main class comment. - size_t rowStart_; // 0 initially - size_t rowEnd_; // the number of row - 1, initially - size_t blockStart_; // 0 initially - -public: - /** Construct from an empty matrix (asserts that the matrix is empty) */ - VerticalBlockView(FullMatrix& matrix) : - matrix_(matrix), rowStart_(0), rowEnd_(matrix_.rows()), blockStart_(0) { - fillOffsets((size_t*)0, (size_t*)0); - assertInvariants(); - } - - /** - * Construct from a non-empty matrix and copy the block structure from - * another block view. - */ - template - VerticalBlockView(FullMatrix& matrix, const RHS& rhs) : - matrix_(matrix) { - if((size_t) matrix_.rows() != rhs.rows() || (size_t) matrix_.cols() != rhs.cols()) - throw std::invalid_argument( - "In VerticalBlockView<>(FullMatrix& matrix, const RHS& rhs), matrix and rhs must\n" - "already be of the same size. If not, construct the VerticalBlockView from an\n" - "empty matrix and then use copyStructureFrom(const RHS& rhs) to resize the matrix\n" - "and set up the block structure."); - copyStructureFrom(rhs); - assertInvariants(); - } - - /** Construct from iterators over the sizes of each vertical block */ - template - VerticalBlockView(FullMatrix& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim) : - matrix_(matrix), rowStart_(0), rowEnd_(matrix_.rows()), blockStart_(0) { - fillOffsets(firstBlockDim, lastBlockDim); - assertInvariants(); - } - - /** - * Construct from a vector of the sizes of each vertical block, resize the - * matrix so that its height is matrixNewHeight and its width fits the given - * block dimensions. - */ - template - VerticalBlockView(FullMatrix& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim, size_t matrixNewHeight) : - matrix_(matrix), rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0) { - fillOffsets(firstBlockDim, lastBlockDim); - matrix_.resize(matrixNewHeight, variableColOffsets_.back()); - assertInvariants(); - } - - /** Row size - */ - size_t rows() const { assertInvariants(); return rowEnd_ - rowStart_; } - - /** Column size - */ - size_t cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } - - - /** Block count - */ - size_t nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } - - - /** Access a single block in the underlying matrix with read/write access */ - inline Block operator()(size_t block) { - return range(block, block+1); - } - - /** Access a const block view */ - inline const constBlock operator()(size_t block) const { - return range(block, block+1); - } - - /** access ranges of blocks at a time */ - inline Block range(size_t startBlock, size_t endBlock) { - assertInvariants(); - size_t actualStartBlock = startBlock + blockStart_; - size_t actualEndBlock = endBlock + blockStart_; - if(startBlock != 0 && endBlock != 0) - checkBlock(actualStartBlock); - assert(actualEndBlock < variableColOffsets_.size()); - const size_t& startCol = variableColOffsets_[actualStartBlock]; - const size_t& endCol = variableColOffsets_[actualEndBlock]; - return matrix_.block(rowStart_, startCol, rowEnd_-rowStart_, endCol-startCol); - } - - inline const constBlock range(size_t startBlock, size_t endBlock) const { - assertInvariants(); - size_t actualStartBlock = startBlock + blockStart_; - size_t actualEndBlock = endBlock + blockStart_; - if(startBlock != 0 && endBlock != 0) - checkBlock(actualStartBlock); - assert(actualEndBlock < variableColOffsets_.size()); - const size_t& startCol = variableColOffsets_[actualStartBlock]; - const size_t& endCol = variableColOffsets_[actualEndBlock]; - return ((const FullMatrix&)matrix_).block(rowStart_, startCol, rowEnd_-rowStart_, endCol-startCol); - } - - /** 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()); - } - - /** get a single column out of a block */ - Column column(size_t block, size_t columnOffset) { - assertInvariants(); - size_t actualBlock = block + blockStart_; - checkBlock(actualBlock); - assert(variableColOffsets_[actualBlock] + columnOffset < variableColOffsets_[actualBlock+1]); - return matrix_.col(variableColOffsets_[actualBlock] + columnOffset).segment(rowStart_, rowEnd_-rowStart_); - } - - /** get a single column out of a block */ - const constColumn column(size_t block, size_t columnOffset) const { - assertInvariants(); - size_t actualBlock = block + blockStart_; - checkBlock(actualBlock); - assert(variableColOffsets_[actualBlock] + columnOffset < (size_t) matrix_.cols()); - return ((const FullMatrix&)matrix_).col(variableColOffsets_[actualBlock] + columnOffset).segment(rowStart_, rowEnd_-rowStart_); - } - - size_t offset(size_t block) const { - assertInvariants(); - size_t actualBlock = block + blockStart_; - checkBlock(actualBlock); - return variableColOffsets_[actualBlock]; - } - - /** Get or set the apparent first row of the underlying matrix for all operations */ - size_t& rowStart() { return rowStart_; } - - /** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ - size_t& rowEnd() { return rowEnd_; } - - /** Get or set the apparent first block for all operations */ - size_t& firstBlock() { return blockStart_; } - - /** Get the apparent first row of the underlying matrix for all operations */ - size_t rowStart() const { return rowStart_; } - - /** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ - size_t rowEnd() const { return rowEnd_; } - - /** Get the apparent first block for all operations */ - size_t firstBlock() const { return blockStart_; } - - /** access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */ - const FullMatrix& fullMatrix() const { return matrix_; } - - /** - * 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. - */ - template - void copyStructureFrom(const RHS& rhs) { - if((size_t) matrix_.rows() != (size_t) rhs.rows() || (size_t) matrix_.cols() != (size_t) rhs.cols()) - matrix_.resize(rhs.rows(), rhs.cols()); - if(rhs.blockStart_ == 0) - variableColOffsets_ = rhs.variableColOffsets_; - else { - variableColOffsets_.resize(rhs.nBlocks() + 1); - variableColOffsets_[0] = 0; - size_t j=0; - assert(rhs.variableColOffsets_.begin()+rhs.blockStart_ < rhs.variableColOffsets_.end()-1); - for(std::vector::const_iterator off=rhs.variableColOffsets_.begin()+rhs.blockStart_; off!=rhs.variableColOffsets_.end()-1; ++off) { - variableColOffsets_[j+1] = variableColOffsets_[j] + (*(off+1) - *off); - ++ j; - } - } - rowStart_ = 0; - rowEnd_ = matrix_.rows(); - blockStart_ = 0; - assertInvariants(); - } - - /** Copy the block struture and matrix data, resizing the underlying matrix - * in the process. This can deal with assigning between different types of - * underlying matrices, as long as the matrices themselves are assignable. - * To avoid creating a temporary matrix this assumes no aliasing, i.e. that - * no part of the underlying matrices refer to the same memory! - * - * 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. - */ - template - VerticalBlockView& assignNoalias(const RHS& rhs) { - copyStructureFrom(rhs); - matrix_.noalias() = rhs.full(); - return *this; - } - - /** Swap the contents of the underlying matrix and the block information with - * another VerticalBlockView. - */ - void swap(VerticalBlockView& other) { - matrix_.swap(other.matrix_); - variableColOffsets_.swap(other.variableColOffsets_); - std::swap(rowStart_, other.rowStart_); - std::swap(rowEnd_, other.rowEnd_); - std::swap(blockStart_, other.blockStart_); - assertInvariants(); - other.assertInvariants(); - } - -protected: - void assertInvariants() const { - assert((size_t) matrix_.cols() == variableColOffsets_.back()); - assert(blockStart_ < variableColOffsets_.size()); - assert(rowStart_ <= (size_t) matrix_.rows()); - assert(rowEnd_ <= (size_t) matrix_.rows()); - assert(rowStart_ <= rowEnd_); - } - - void checkBlock(size_t block) const { - assert((size_t) matrix_.cols() == variableColOffsets_.back()); - assert(block < variableColOffsets_.size()-1); - assert(variableColOffsets_[block] < (size_t) matrix_.cols() && variableColOffsets_[block+1] <= (size_t) matrix_.cols()); - } - - template - void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) { - variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1); - variableColOffsets_[0] = 0; - size_t j=0; - for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { - variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; - ++ j; - } - } - - template friend class SymmetricBlockView; - template friend class VerticalBlockView; - -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_); - } -}; - -/** - * This class stores a *reference* to a matrix and allows it to be accessed as - * a collection of blocks. It also provides for accessing individual - * columns from those blocks. When constructed or resized, the caller must - * provide the dimensions of the blocks, as well as an underlying matrix - * storage object. This class will resize the underlying matrix such that it - * is consistent with the given block dimensions. - * - * This class uses a symmetric block structure. The underlying matrix does not - * necessarily need to be symmetric. - * - * 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 - */ -template -class SymmetricBlockView { -public: - typedef MATRIX FullMatrix; - typedef Eigen::Block Block; - typedef Eigen::Block constBlock; - typedef typename FullMatrix::ColXpr::SegmentReturnType Column; - typedef typename FullMatrix::ConstColXpr::ConstSegmentReturnType constColumn; - -private: - static FullMatrix matrixTemp_; // just for finding types - -protected: - FullMatrix& matrix_; // the reference to the full matrix - std::vector variableColOffsets_; // the starting columns of each block (0-based) - - // Changes apparent matrix view, see main class comment. - size_t blockStart_; // 0 initially - -public: - /** Construct from an empty matrix (asserts that the matrix is empty) */ - SymmetricBlockView(FullMatrix& matrix) : - matrix_(matrix), blockStart_(0) { - fillOffsets((size_t*)0, (size_t*)0); - assertInvariants(); - } - - /** Construct from iterators over the sizes of each block */ - template - SymmetricBlockView(FullMatrix& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim) : - matrix_(matrix), blockStart_(0) { - fillOffsets(firstBlockDim, lastBlockDim); - assertInvariants(); - } - - /** - * Modify the size and structure of the underlying matrix and this block - * view. If 'preserve' is true, the underlying matrix data will be copied if - * the matrix size changes, otherwise the new data will be uninitialized. - */ - template - void resize(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool preserve) { - blockStart_ = 0; - fillOffsets(firstBlockDim, lastBlockDim); - if (preserve) - matrix_.conservativeResize(variableColOffsets_.back(), variableColOffsets_.back()); - else - matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); - } - - /** Row size - */ - size_t rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } - - /** Column size - */ - size_t cols() const { return rows(); } - - - /** Block count - */ - size_t nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } - - - Block operator()(size_t i_block, size_t j_block) { - return range(i_block, i_block+1, j_block, j_block+1); - } - - constBlock operator()(size_t i_block, size_t j_block) const { - return range(i_block, i_block+1, j_block, j_block+1); - } - - Block range(size_t i_startBlock, size_t i_endBlock, size_t j_startBlock, size_t j_endBlock) { - assertInvariants(); - size_t i_actualStartBlock = i_startBlock + blockStart_; - size_t i_actualEndBlock = i_endBlock + blockStart_; - size_t j_actualStartBlock = j_startBlock + blockStart_; - size_t j_actualEndBlock = j_endBlock + blockStart_; - checkBlock(i_actualStartBlock); - checkBlock(j_actualStartBlock); - assert(i_actualEndBlock < variableColOffsets_.size()); - assert(j_actualEndBlock < variableColOffsets_.size()); - return matrix_.block( - variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock], - variableColOffsets_[j_actualEndBlock]-variableColOffsets_[j_actualStartBlock]); - } - - constBlock range(size_t i_startBlock, size_t i_endBlock, size_t j_startBlock, size_t j_endBlock) const { - assertInvariants(); - size_t i_actualStartBlock = i_startBlock + blockStart_; - size_t i_actualEndBlock = i_endBlock + blockStart_; - size_t j_actualStartBlock = j_startBlock + blockStart_; - size_t j_actualEndBlock = j_endBlock + blockStart_; - checkBlock(i_actualStartBlock); - checkBlock(j_actualStartBlock); - assert(i_actualEndBlock < variableColOffsets_.size()); - assert(j_actualEndBlock < variableColOffsets_.size()); - return ((const FullMatrix&)matrix_).block( - variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock], - variableColOffsets_[j_actualEndBlock]-variableColOffsets_[j_actualStartBlock]); - } - - Block full() { - return range(0,nBlocks(), 0,nBlocks()); - } - - constBlock full() const { - return range(0,nBlocks(), 0,nBlocks()); - } - - /** access to full matrix */ - const FullMatrix& fullMatrix() const { return matrix_; } - - Column column(size_t i_block, size_t j_block, size_t columnOffset) { - assertInvariants(); - size_t i_actualBlock = i_block + blockStart_; - size_t j_actualBlock = j_block + blockStart_; - checkBlock(i_actualBlock); - checkBlock(j_actualBlock); - assert(i_actualBlock < variableColOffsets_.size()); - assert(j_actualBlock < variableColOffsets_.size()); - assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]); - - return matrix_.col( - variableColOffsets_[j_actualBlock] + columnOffset).segment( - variableColOffsets_[i_actualBlock], - variableColOffsets_[i_actualBlock+1]-variableColOffsets_[i_actualBlock]); - } - - constColumn column(size_t i_block, size_t j_block, size_t columnOffset) const { - assertInvariants(); - size_t i_actualBlock = i_block + blockStart_; - size_t j_actualBlock = j_block + blockStart_; - checkBlock(i_actualBlock); - checkBlock(j_actualBlock); - assert(i_actualBlock < variableColOffsets_.size()); - assert(j_actualBlock < variableColOffsets_.size()); - assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]); - - return ((const FullMatrix&)matrix_).col( - variableColOffsets_[j_actualBlock] + columnOffset).segment( - variableColOffsets_[i_actualBlock], - variableColOffsets_[i_actualBlock+1]-variableColOffsets_[i_actualBlock]); -// assertInvariants(); -// size_t j_actualBlock = j_block + blockStart_; -// assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]); -// constBlock blockMat(operator()(i_block, j_block)); -// return constColumn(blockMat, columnOffset); - } - - Column rangeColumn(size_t i_startBlock, size_t i_endBlock, size_t j_block, size_t columnOffset) { - assertInvariants(); - - size_t i_actualStartBlock = i_startBlock + blockStart_; - size_t i_actualEndBlock = i_endBlock + blockStart_; - size_t j_actualStartBlock = j_block + blockStart_; - checkBlock(i_actualStartBlock); - checkBlock(j_actualStartBlock); - assert(i_actualEndBlock < variableColOffsets_.size()); - assert(variableColOffsets_[j_actualStartBlock] + columnOffset < variableColOffsets_[j_actualStartBlock+1]); - - return matrix_.col( - variableColOffsets_[j_actualStartBlock] + columnOffset).segment( - variableColOffsets_[i_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]); - } - - constColumn rangeColumn(size_t i_startBlock, size_t i_endBlock, size_t j_block, size_t columnOffset) const { - assertInvariants(); - - size_t i_actualStartBlock = i_startBlock + blockStart_; - size_t i_actualEndBlock = i_endBlock + blockStart_; - size_t j_actualStartBlock = j_block + blockStart_; - checkBlock(i_actualStartBlock); - checkBlock(j_actualStartBlock); - assert(i_actualEndBlock < variableColOffsets_.size()); - assert(variableColOffsets_[j_actualStartBlock] + columnOffset < variableColOffsets_[j_actualStartBlock+1]); - - return ((const FullMatrix&)matrix_).col( - variableColOffsets_[j_actualStartBlock] + columnOffset).segment( - variableColOffsets_[i_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]); - } - - size_t offset(size_t block) const { - assertInvariants(); - size_t actualBlock = block + blockStart_; - checkBlock(actualBlock); - return variableColOffsets_[actualBlock]; - } - - size_t& blockStart() { return blockStart_; } - size_t blockStart() const { return blockStart_; } - - /** Copy the block structure and resize the underlying matrix, but do not - * copy the matrix data. If blockStart() has been modified, this copies the - * structure of the corresponding matrix view. In the destination - * SymmetricBlockView, startBlock() will thus be 0 and the underlying matrix - * will be the size of the view of the source matrix. - */ - template - void copyStructureFrom(const RHS& rhs) { - matrix_.resize(rhs.cols(), rhs.cols()); - if(rhs.blockStart_ == 0) - variableColOffsets_ = rhs.variableColOffsets_; - else { - variableColOffsets_.resize(rhs.nBlocks() + 1); - variableColOffsets_[0] = 0; - size_t j=0; - assert(rhs.variableColOffsets_.begin()+rhs.blockStart_ < rhs.variableColOffsets_.end()-1); - for(std::vector::const_iterator off=rhs.variableColOffsets_.begin()+rhs.blockStart_; off!=rhs.variableColOffsets_.end()-1; ++off) { - variableColOffsets_[j+1] = variableColOffsets_[j] + (*(off+1) - *off); - ++ j; - } - } - blockStart_ = 0; - assertInvariants(); - } - - /** Copy the block struture and matrix data, resizing the underlying matrix - * in the process. This can deal with assigning between different types of - * underlying matrices, as long as the matrices themselves are assignable. - * To avoid creating a temporary matrix this assumes no aliasing, i.e. that - * no part of the underlying matrices refer to the same memory! - * - * If blockStart() has been modified, this copies the structure of the - * corresponding matrix view. In the destination SymmetricBlockView, - * startBlock() will thus be 0 and the underlying matrix will be the size - * of the view of the source matrix. - */ - template - SymmetricBlockView& assignNoalias(const SymmetricBlockView& rhs) { - copyStructureFrom(rhs); - matrix_.noalias() = rhs.full(); - return *this; - } - - /** Swap the contents of the underlying matrix and the block information with - * another VerticalBlockView. - */ - void swap(SymmetricBlockView& other) { - matrix_.swap(other.matrix_); - variableColOffsets_.swap(other.variableColOffsets_); - std::swap(blockStart_, other.blockStart_); - assertInvariants(); - other.assertInvariants(); - } - -protected: - void assertInvariants() const { - assert(matrix_.rows() == matrix_.cols()); - assert((size_t) matrix_.cols() == variableColOffsets_.back()); - assert(blockStart_ < variableColOffsets_.size()); - } - - void checkBlock(size_t block) const { - assert(matrix_.rows() == matrix_.cols()); - assert((size_t) matrix_.cols() == variableColOffsets_.back()); - assert(block < variableColOffsets_.size()-1); - assert(variableColOffsets_[block] < (size_t) matrix_.cols() && variableColOffsets_[block+1] <= (size_t) matrix_.cols()); - } - - template - void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) { - variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1); - variableColOffsets_[0] = 0; - size_t j=0; - for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { - variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; - ++ j; - } - } - - template friend class SymmetricBlockView; - template friend class VerticalBlockView; - -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_); - } -}; - - -} diff --git a/gtsam/base/tests/testBlockMatrices.cpp b/gtsam/base/tests/testBlockMatrices.cpp deleted file mode 100644 index 3950d561a..000000000 --- a/gtsam/base/tests/testBlockMatrices.cpp +++ /dev/null @@ -1,133 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testBlockMatrices - * @author Alex Cunningham - */ - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -TEST(testBlockMatrices, jacobian_factor1) { - typedef Matrix AbMatrix; - typedef VerticalBlockView BlockAb; - - AbMatrix matrix; // actual matrix - empty to start with - - // from JacobianFactor::Constructor - one variable - Matrix A1 = Matrix_(2,3, - 1., 2., 3., - 4., 5., 6.); - Vector b = Vector_(2, 7., 8.); - size_t dims[] = { A1.cols(), 1}; - - // build the structure - BlockAb Ab(matrix, dims, dims+2, b.size()); - - // add a matrix and get back out - Ab(0) = A1; - EXPECT(assert_equal(A1, Ab(0))); - - // add vector to the system - Ab.column(1, 0) = b; - EXPECT(assert_equal(A1, Ab(0))); - EXPECT(assert_equal(b, Ab.column(1,0))); - - // examine matrix contents - EXPECT_LONGS_EQUAL(2, Ab.nBlocks()); - Matrix expFull = Matrix_(2, 4, - 1., 2., 3., 7., - 4., 5., 6., 8.); - Matrix actFull = Ab.full(); - EXPECT(assert_equal(expFull, actFull)); -} - -/* ************************************************************************* */ -TEST(testBlockMatrices, jacobian_factor2) { - typedef Matrix AbMatrix; - typedef VerticalBlockView BlockAb; - - AbMatrix matrix; // actual matrix - empty to start with - - // from JacobianFactor::Constructor - two variables - Matrix A1 = Matrix_(2,3, - 1., 2., 3., - 4., 5., 6.); - Matrix A2 = Matrix_(2,1, - 10., - 11.); - Vector b = Vector_(2, 7., 8.); - size_t dims[] = { A1.cols(), A2.cols(), 1}; - - // build the structure - BlockAb Ab(matrix, dims, dims+3, b.size()); - - // add blocks - Ab(0) = A1; - Ab(1) = A2; - EXPECT(assert_equal(A1, Ab(0))); - EXPECT(assert_equal(A2, Ab(1))); - - // add vector to the system - Ab.column(2, 0) = b; - EXPECT(assert_equal(A1, Ab(0))); - EXPECT(assert_equal(A2, Ab(1))); - EXPECT(assert_equal(b, Ab.column(2,0))); - - // examine matrix contents - EXPECT_LONGS_EQUAL(3, Ab.nBlocks()); - Matrix expFull = Matrix_(2, 5, - 1., 2., 3., 10., 7., - 4., 5., 6., 11., 8.); - Matrix actFull = Ab.full(); - EXPECT(assert_equal(expFull, actFull)); -} - -/* ************************************************************************* */ -TEST(testBlockMatrices, hessian_factor1) { - typedef Matrix InfoMatrix; - typedef SymmetricBlockView BlockInfo; - - Matrix expected_full = Matrix_(3, 3, - 3.0, 5.0, -8.0, - 0.0, 6.0, -9.0, - 0.0, 0.0, 10.0); - - Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Vector g = Vector_(2, -8.0, -9.0); - double f = 10.0; - - size_t dims[] = { G.rows(), 1 }; - InfoMatrix fullMatrix = zeros(G.rows() + 1, G.rows() + 1); - BlockInfo infoView(fullMatrix, dims, dims+2); - infoView(0,0) = G; - infoView.column(0,1,0) = g; - infoView(1,1)(0,0) = f; - - EXPECT_LONGS_EQUAL(0, infoView.blockStart()); - EXPECT_LONGS_EQUAL(2, infoView.nBlocks()); - EXPECT(assert_equal(InfoMatrix(expected_full), fullMatrix)); - EXPECT(assert_equal(InfoMatrix(G), infoView.range(0, 1, 0, 1))) - EXPECT_DOUBLES_EQUAL(f, infoView(1, 1)(0,0), 1e-10); - - EXPECT(assert_equal(g, Vector(infoView.rangeColumn(0, 1, 1, 0)))); - EXPECT(assert_equal(g, Vector(((const BlockInfo)infoView).rangeColumn(0, 1, 1, 0)))); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index a4fe3f405..fbcfcf15b 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -18,7 +18,6 @@ #pragma once -#include #include #include #include diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index 1c486f3a0..4b95c65b4 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -37,16 +37,13 @@ LinearizedGaussianFactor::LinearizedGaussianFactor( /* ************************************************************************* */ // LinearizedJacobianFactor /* ************************************************************************* */ -LinearizedJacobianFactor::LinearizedJacobianFactor() : Ab_(matrix_) { +LinearizedJacobianFactor::LinearizedJacobianFactor() { } /* ************************************************************************* */ LinearizedJacobianFactor::LinearizedJacobianFactor( const JacobianFactor::shared_ptr& jacobian, const Values& lin_points) -: Base(jacobian, lin_points), Ab_(matrix_) { - - // Get the Ab matrix from the Jacobian factor, with any covariance baked in - AbMatrix fullMatrix = jacobian->augmentedJacobian(); +: Base(jacobian, lin_points) { // Create the dims array size_t *dims = (size_t *)alloca(sizeof(size_t) * (jacobian->size() + 1)); @@ -57,8 +54,9 @@ LinearizedJacobianFactor::LinearizedJacobianFactor( dims[index] = 1; // Update the BlockInfo accessor - BlockAb Ab(fullMatrix, dims, dims+jacobian->size()+1); - Ab.swap(Ab_); + Ab_ = VerticalBlockMatrix(dims, dims+jacobian->size()+1, jacobian->rows()); + // Get the Ab matrix from the Jacobian factor, with any covariance baked in + Ab_.matrix() = jacobian->augmentedJacobian(); } /* ************************************************************************* */ @@ -136,16 +134,13 @@ Vector LinearizedJacobianFactor::error_vector(const Values& c) const { /* ************************************************************************* */ // LinearizedHessianFactor /* ************************************************************************* */ -LinearizedHessianFactor::LinearizedHessianFactor() : info_(matrix_) { +LinearizedHessianFactor::LinearizedHessianFactor() { } /* ************************************************************************* */ LinearizedHessianFactor::LinearizedHessianFactor( const HessianFactor::shared_ptr& hessian, const Values& lin_points) -: Base(hessian, lin_points), info_(matrix_) { - - // Copy the augmented matrix holding G, g, and f - Matrix fullMatrix = hessian->info(); +: Base(hessian, lin_points) { // Create the dims array size_t *dims = (size_t*)alloca(sizeof(size_t)*(hessian->size() + 1)); @@ -156,8 +151,9 @@ LinearizedHessianFactor::LinearizedHessianFactor( dims[index] = 1; // Update the BlockInfo accessor - BlockInfo infoMatrix(fullMatrix, dims, dims+hessian->size()+1); - infoMatrix.swap(info_); + info_ = SymmetricBlockMatrix(dims, dims+hessian->size()+1); + // Copy the augmented matrix holding G, g, and f + info_.matrix() = hessian->info(); } /* ************************************************************************* */ @@ -220,11 +216,6 @@ double LinearizedHessianFactor::error(const Values& c) const { boost::shared_ptr LinearizedHessianFactor::linearize(const Values& c) const { - // Make a copy of the info matrix - Matrix newMatrix; - SymmetricBlockView newInfo(newMatrix); - newInfo.assignNoalias(info_); - // Construct an error vector in key-order from the Values Vector dx = zero(dim()); size_t index = 0; @@ -244,15 +235,15 @@ LinearizedHessianFactor::linearize(const Values& c) const { //newInfo.rangeColumn(0, this->size(), this->size(), 0) -= squaredTerm().selfadjointView() * dx; Vector g = linearTerm() - squaredTerm().selfadjointView() * dx; std::vector gs; - for(size_t i = 0; i < info_.nBlocks()-1; ++i) { + for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) { gs.push_back(g.segment(info_.offset(i), info_.offset(i+1) - info_.offset(i))); } // G2 = G1 // Do Nothing std::vector Gs; - for(size_t i = 0; i < info_.nBlocks()-1; ++i) { - for(size_t j = i; j < info_.nBlocks()-1; ++j) { + for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) { + for(DenseIndex j = i; j < info_.nBlocks()-1; ++j) { Gs.push_back(info_(i,j)); } } diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 1c8d994f4..9e2319ae6 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -22,7 +22,6 @@ #include #include #include -#include namespace gtsam { @@ -85,12 +84,10 @@ public: /** shared pointer for convenience */ typedef boost::shared_ptr shared_ptr; - typedef Matrix AbMatrix; - typedef VerticalBlockView BlockAb; - typedef BlockAb::Block ABlock; - typedef BlockAb::constBlock constABlock; - typedef BlockAb::Column BVector; - typedef BlockAb::constColumn constBVector; + typedef VerticalBlockMatrix::Block ABlock; + typedef VerticalBlockMatrix::constBlock constABlock; + typedef VerticalBlockMatrix::Block::ColXpr BVector; + typedef VerticalBlockMatrix::constBlock::ConstColXpr constBVector; protected: @@ -99,8 +96,7 @@ protected: // KeyMatrixMap matrices_; // Vector b_; - AbMatrix matrix_; // the full matrix corresponding to the factor - BlockAb Ab_; // the block view of the full matrix + VerticalBlockMatrix Ab_; // the block view of the full matrix public: @@ -129,7 +125,7 @@ public: virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; // access functions - const constBVector b() const { return Ab_.column(size(), 0); } + const constBVector b() const { return Ab_(size()).col(0); } const constABlock A() const { return Ab_.range(0, size()); }; const constABlock A(Key key) const { return Ab_(std::find(begin(), end(), key) - begin()); } @@ -156,7 +152,6 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("LinearizedJacobianFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(matrix_); ar & BOOST_SERIALIZATION_NVP(Ab_); } }; @@ -179,17 +174,15 @@ public: typedef boost::shared_ptr shared_ptr; /** hessian block data types */ - typedef Matrix InfoMatrix; ///< The full augmented Hessian - typedef SymmetricBlockView BlockInfo; ///< A blockwise view of the Hessian - typedef BlockInfo::Block Block; ///< A block from the Hessian matrix - typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version) - typedef BlockInfo::Column Column; ///< A column containing the linear term h - typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version) + typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix + typedef SymmetricBlockMatrix::constBlock constBlock; ///< A block from the Hessian matrix (const version) + typedef SymmetricBlockMatrix::Block::ColXpr Column; ///< A column containing the linear term h + typedef SymmetricBlockMatrix::constBlock::ColXpr constColumn; ///< A column containing the linear term h (const version) protected: - InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1] - BlockInfo info_; ///< The block view of the full information matrix. + SymmetricBlockMatrix info_; ///< The block view of the full information matrix, s.t. the quadratic + /// error is 0.5*[x -1]'*H*[x -1] public: @@ -227,11 +220,11 @@ public: * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can * use, for example, begin() + 2 to get the 3rd variable in this factor. * @return The linear term \f$ g \f$ */ - constColumn linearTerm(const_iterator j) const { return info_.column(j - this->begin(), this->size(), 0); } + constColumn linearTerm(const_iterator j) const { return info_(j - this->begin(), this->size()).col(0); } /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ - constColumn linearTerm() const { return info_.rangeColumn(0, this->size(), this->size(), 0); }; + constColumn linearTerm() const { return info_.range(0, this->size(), this->size(), this->size() + 1).col(0); }; /** Return a view of the block at (j1,j2) of the upper-triangular part of the * squared term \f$ H \f$, no data is copied. See HessianFactor class documentation @@ -253,7 +246,7 @@ public: /** get the dimension of the factor (number of rows on linearization) */ - size_t dim() const { return matrix_.rows() - 1; }; + size_t dim() const { return info_.rows() - 1; }; /** Calculate the error of the factor */ double error(const Values& c) const; @@ -272,7 +265,6 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("LinearizedHessianFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(matrix_); ar & BOOST_SERIALIZATION_NVP(info_); } };