From 5f87cbea7fcf4e87b23680ea8b6bb5142b7cd28f Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 26 Nov 2013 23:49:53 +0000 Subject: [PATCH] New SymmetricBlockMatrix interface - should be safer as all exposed functions use symmetric matrix expressions and the internal uninitialized values below the diagonal are never exposed. --- gtsam/base/SymmetricBlockMatrix.cpp | 28 ++ gtsam/base/SymmetricBlockMatrix.h | 218 +++++++---- gtsam/base/SymmetricBlockMatrixBlockExpr.h | 337 ++++++++++++++++++ gtsam/base/tests/testSymmetricBlockMatrix.cpp | 188 ++++++++++ gtsam/linear/HessianFactor.cpp | 85 ++--- gtsam/linear/HessianFactor.h | 19 +- .../tests/testHessianFactorUnordered.cpp | 15 +- gtsam/nonlinear/LinearContainerFactor.cpp | 5 +- gtsam/nonlinear/Marginals.cpp | 4 +- gtsam/nonlinear/Marginals.h | 2 +- gtsam/slam/tests/testAntiFactor.cpp | 2 +- gtsam_unstable/nonlinear/LinearizedFactor.cpp | 14 +- gtsam_unstable/nonlinear/LinearizedFactor.h | 10 +- 13 files changed, 771 insertions(+), 156 deletions(-) create mode 100644 gtsam/base/SymmetricBlockMatrixBlockExpr.h create mode 100644 gtsam/base/tests/testSymmetricBlockMatrix.cpp diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index beabc7755..98fe1be5d 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -18,6 +18,8 @@ #include #include +#include +#include namespace gtsam { @@ -47,4 +49,30 @@ namespace gtsam { 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; + } } diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 13b493f36..14014898a 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { @@ -36,12 +37,12 @@ namespace gtsam { * (except re-setting firstBlock()). * * @addtogroup base */ - class SymmetricBlockMatrix + class GTSAM_EXPORT SymmetricBlockMatrix { public: typedef SymmetricBlockMatrix This; - typedef Eigen::Block Block; - typedef Eigen::Block constBlock; + typedef SymmetricBlockMatrixBlockExpr Block; + typedef SymmetricBlockMatrixBlockExpr constBlock; protected: Matrix matrix_; ///< The full matrix @@ -50,7 +51,7 @@ namespace gtsam { DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment. public: - /** Construct from an empty matrix (asserts that the matrix is empty) */ + /// Construct from an empty matrix (asserts that the matrix is empty) SymmetricBlockMatrix() : blockStart_(0) { @@ -58,7 +59,7 @@ namespace gtsam { assertInvariants(); } - /** Construct from a container of the sizes of each block. */ + /// Construct from a container of the sizes of each block. template SymmetricBlockMatrix(const CONTAINER& dimensions) : blockStart_(0) @@ -68,8 +69,7 @@ namespace gtsam { assertInvariants(); } - /** - * Construct from iterator over the sizes of each vertical block. */ + /// Construct from iterator over the sizes of each vertical block. template SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim) : blockStart_(0) @@ -79,11 +79,13 @@ namespace gtsam { assertInvariants(); } - /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ + /// Construct from a container of the sizes of each vertical block and a pre-prepared matrix. template SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) : - matrix_(matrix), blockStart_(0) + 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."); @@ -92,118 +94,195 @@ namespace gtsam { 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. */ + /// 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. */ + /// 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); - /** Row size */ + /// Row size DenseIndex rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } - /** Column size */ + /// Column size DenseIndex cols() const { return rows(); } - - /** Block count */ + /// 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 range(i_block, i_block+1, j_block, j_block+1); + 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 range(i_block, i_block+1, j_block, j_block+1); + 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(); - DenseIndex i_actualStartBlock = i_startBlock + blockStart_; - DenseIndex i_actualEndBlock = i_endBlock + blockStart_; - DenseIndex j_actualStartBlock = j_startBlock + blockStart_; - 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()); - } - return matrix_.block( - variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], - variableColOffsets_[i_actualEndBlock] - variableColOffsets_[i_actualStartBlock], - variableColOffsets_[j_actualEndBlock] - variableColOffsets_[j_actualStartBlock]); + 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(); - DenseIndex i_actualStartBlock = i_startBlock + blockStart_; - DenseIndex i_actualEndBlock = i_endBlock + blockStart_; - DenseIndex j_actualStartBlock = j_startBlock + blockStart_; - DenseIndex j_actualEndBlock = j_endBlock + blockStart_; - 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()); - } - return matrix_.block( - variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], - variableColOffsets_[i_actualEndBlock] - variableColOffsets_[i_actualStartBlock], - variableColOffsets_[j_actualEndBlock] - variableColOffsets_[j_actualStartBlock]); + 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 range(0,nBlocks(), 0,nBlocks()); + Block full() + { + return Block(*this, 0, nBlocks(), 0); } /** Return the full matrix, *not* including any portions excluded by firstBlock(). */ - constBlock full() const { - return range(0,nBlocks(), 0,nBlocks()); + constBlock full() const + { + return constBlock(*this, 0, nBlocks(), 0); } /** Access to full matrix, including any portions excluded by firstBlock() to other operations. */ - const Matrix& matrix() const { return matrix_; } + Eigen::SelfAdjointView matrix() const + { + return matrix_; + } /** Access to full matrix, including any portions excluded by firstBlock() to other operations. */ - Matrix& matrix() { return matrix_; } + Eigen::SelfAdjointView matrix() + { + return matrix_; + } - DenseIndex offset(DenseIndex block) const { + /// 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 { + void assertInvariants() const + { assert(matrix_.rows() == matrix_.cols()); assert(matrix_.cols() == variableColOffsets_.back()); assert(blockStart_ < (DenseIndex)variableColOffsets_.size()); } - void checkBlock(DenseIndex block) const { + 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) { + void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) + { variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1); variableColOffsets_[0] = 0; DenseIndex j=0; @@ -214,6 +293,7 @@ namespace gtsam { } friend class VerticalBlockMatrix; + template friend class SymmetricBlockMatrixBlockExpr; private: /** Serialization function */ @@ -226,5 +306,13 @@ namespace gtsam { } }; + /* ************************************************************************* */ + class CholeskyFailed : public gtsam::ThreadsafeException + { + public: + CholeskyFailed() throw() {} + virtual ~CholeskyFailed() throw() {} + }; } + diff --git a/gtsam/base/SymmetricBlockMatrixBlockExpr.h b/gtsam/base/SymmetricBlockMatrixBlockExpr.h new file mode 100644 index 000000000..9c7676ae3 --- /dev/null +++ b/gtsam/base/SymmetricBlockMatrixBlockExpr.h @@ -0,0 +1,337 @@ +/* ---------------------------------------------------------------------------- + +* 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 SymmetricBlockMatrixBlockExpr.h +* @brief Matrix expression for a block of a SymmetricBlockMatrix +* @author Richard Roberts +* @date Nov 20, 2013 +*/ +#pragma once + +#include + +namespace gtsam { template class SymmetricBlockMatrixBlockExpr; } +namespace gtsam { class SymmetricBlockMatrix; } + +// traits class for Eigen expressions +namespace Eigen +{ + namespace internal + { + template + struct traits > : + public traits::type> + { + }; + } +} + +namespace gtsam +{ + /// A matrix expression that references a single block of a SymmetricBlockMatrix. Depending on + /// the position of the block, this expression will behave either as a regular matrix block, a + /// transposed matrix block, or a symmetric matrix block. The only reason this class is templated + /// on SymmetricBlockMatrixType is to allow for both const and non-const references. + template + class SymmetricBlockMatrixBlockExpr : public Eigen::EigenBase > + { + protected: + SymmetricBlockMatrixType& xpr_; ///< The referenced SymmetricBlockMatrix + DenseIndex densei_; ///< The scalar indices of the referenced block + DenseIndex densej_; ///< The scalar indices of the referenced block + DenseIndex denseRows_; ///< The scalar size of the referenced block + DenseIndex denseCols_; ///< The scalar size of the referenced block + enum BlockType { Plain, SelfAdjoint, Transposed } blockType_; ///< The type of the referenced block, as determined by the block position + typedef SymmetricBlockMatrixBlockExpr This; + + public: + // Typedefs and constants used in Eigen + typedef typename const_selector::Scalar&, typename Eigen::internal::traits::Scalar>::type ScalarRef; + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::internal::traits::Index Index; + static const Index ColsAtCompileTime = Eigen::Dynamic; + static const Index RowsAtCompileTime = Eigen::Dynamic; + + typedef typename const_selector::type + DenseMatrixType; + + typedef Eigen::Map > OffDiagonal; + typedef Eigen::SelfAdjointView, Eigen::Upper> SelfAdjointView; + typedef Eigen::TriangularView, Eigen::Upper> TriangularView; + + protected: + mutable Eigen::Block myBlock_; + template friend class SymmetricBlockMatrixBlockExpr; + + public: + /// Create a SymmetricBlockMatrixBlockExpr from the specified block of a SymmetricBlockMatrix. + SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index iBlock, Index jBlock) : + xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0)) + { + initIndices(iBlock, jBlock); + } + + /// Create a SymmetricBlockMatrixBlockExpr from the specified range of blocks of a + /// SymmetricBlockMatrix. + SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, + Index firstRowBlock, Index firstColBlock, Index rowBlocks, Index colBlocks) : + xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0)) + { + initIndices(firstRowBlock, firstColBlock, rowBlocks, colBlocks); + } + + /// Create a SymmetricBlockMatrixBlockExpr from the specified range of blocks of a + /// SymmetricBlockMatrix. + SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index firstBlock, Index blocks, char dummy) : + xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0)) + { + initIndices(firstBlock, firstBlock, blocks, blocks); + } + + inline Index rows() const { return blockType_ != Transposed ? denseRows_ : denseCols_; } + inline Index cols() const { return blockType_ != Transposed ? denseCols_ : denseRows_; } + + inline BlockType blockType() const { return blockType_; } + + inline ScalarRef operator()(Index row, Index col) const + { + return coeffInternal(row, col); + } + + inline OffDiagonal knownOffDiagonal() const + { + typedef Eigen::Stride DynamicStride; + + // We can return a Map if we are either on an off-diagonal block, or a block of size 0 or 1 + assert(blockType_ != SelfAdjoint || (denseRows_ <= 1 && denseCols_ <= 1)); + if(blockType_ == Transposed) + { + // Swap the inner and outer stride to produce a transposed Map + Eigen::Block block = const_cast(*this).xpr_.matrix_.block(densei_, densej_, denseRows_, denseCols_); + return Eigen::Map(block.data(), block.cols(), block.rows(), + DynamicStride(block.innerStride(), block.outerStride())); + } + else + { + Eigen::Block block = const_cast(*this).xpr_.matrix_.block(densei_, densej_, denseRows_, denseCols_); + return Eigen::Map(block.data(), block.rows(), block.cols(), + DynamicStride(block.outerStride(), block.innerStride())); + } + } + + inline SelfAdjointView selfadjointView() const + { + assert(blockType_ == SelfAdjoint); + return myBlock_; + } + + inline TriangularView triangularView() const + { + assert(blockType_ == SelfAdjoint); + return myBlock_; + } + + template inline void evalTo(Dest& dst) const + { + // Just try to assign to the object using either a selfadjoint view or a block view + if(blockType_ == SelfAdjoint) + dst = selfadjointView(); + else if(blockType_ == Plain) + dst = myBlock_; + else + dst = myBlock_.transpose(); + } + + //template inline void evalTo(const Eigen::SelfAdjointView& rhs) const + //{ + // if(blockType_ == SelfAdjoint) + // rhs.nestedExpression().triangularView() = triangularView(); + // else + // throw std::invalid_argument("Cannot assign an off-diagonal block to a self-adjoint matrix"); + //} + + //template inline void evalTo(const Eigen::TriangularView& rhs) const + //{ + // if(blockType_ == SelfAdjoint) + // rhs.nestedExpression().triangularView() = triangularView(); + // else + // throw std::invalid_argument("Cannot assign an off-diagonal block to a self-adjoint matrix"); + //} + + template + This& operator=(const Eigen::MatrixBase& rhs) + { + // Just try to assign to the object using either a selfadjoint view or a block view + if(blockType_ == SelfAdjoint) + triangularView() = rhs.derived().template triangularView(); + else if(blockType_ == Plain) + myBlock_ = rhs.derived(); + else + myBlock_.transpose() = rhs.derived(); + return *this; + } + + template + This& operator=(const Eigen::SelfAdjointView& rhs) + { + if(blockType_ == SelfAdjoint) + triangularView() = rhs.nestedExpression().template triangularView(); + else + throw std::invalid_argument("Cannot assign a self-adjoint matrix to an off-diagonal block"); + return *this; + } + + template + This& operator=(const SymmetricBlockMatrixBlockExpr& other) + { + _doAssign(other); + return *this; + } + + This& operator=(const This& other) + { + // This version is required so GCC doesn't synthesize a default operator=. + _doAssign(other); + return *this; + } + + template + This& operator+=(const SymmetricBlockMatrixBlockExpr& other) + { + if(blockType_ == SelfAdjoint) + { + assert(other.blockType() == SelfAdjoint); + triangularView() += other.triangularView().nestedExpression(); + } + else if(blockType_ == Plain) + { + assert(other.blockType() == Plain || other.blockType() == Transposed); + if(other.blockType() == Transposed) + myBlock_ += other.myBlock_.transpose(); + else + myBlock_ += other.myBlock_; + } + else + { + assert(other.blockType() == Plain || other.blockType() == Transposed); + if(other.blockType() == Transposed) + myBlock_.transpose() += other.myBlock_.transpose(); + else + myBlock_.transpose() += other.myBlock_; + } + return *this; + } + + private: + void initIndices(Index iBlock, Index jBlock, Index blockRows = 1, Index blockCols = 1) + { + if(iBlock == jBlock && blockRows == blockCols) + { + densei_ = xpr_.offset(iBlock); + densej_ = densei_; + if(blockRows > 0) + xpr_.checkBlock(iBlock + blockRows - 1); + denseRows_ = xpr_.offsetUnchecked(iBlock + blockRows) - densei_; + if(blockCols > 0) + xpr_.checkBlock(jBlock + blockCols - 1); + denseCols_ = xpr_.offsetUnchecked(jBlock + blockCols) - densej_; + blockType_ = SelfAdjoint; + } + else + { + if(jBlock > iBlock || (iBlock == jBlock && blockCols > blockRows)) + { + densei_ = xpr_.offset(iBlock); + densej_ = xpr_.offset(jBlock); + if(blockRows > 0) + xpr_.checkBlock(iBlock + blockRows - 1); + denseRows_ = xpr_.offsetUnchecked(iBlock + blockRows) - densei_; + if(blockCols > 0) + xpr_.checkBlock(jBlock + blockCols - 1); + denseCols_ = xpr_.offsetUnchecked(jBlock + blockCols) - densej_; + blockType_ = Plain; + } + else + { + densei_ = xpr_.offset(jBlock); + densej_ = xpr_.offset(iBlock); + if(blockCols > 0) + xpr_.checkBlock(jBlock + blockCols - 1); + denseRows_ = xpr_.offsetUnchecked(jBlock + blockCols) - densei_; + if(blockRows > 0) + xpr_.checkBlock(iBlock + blockRows - 1); + denseCols_ = xpr_.offsetUnchecked(iBlock + blockRows) - densej_; + blockType_ = Transposed; + } + + // Validate that the block does not cross below the diagonal (the indices have already been + // flipped above the diagonal for ranges starting below the diagonal). + if(densei_ + denseRows_ > densej_ + 1) + throw std::invalid_argument("Off-diagonal block ranges may not cross the diagonal"); + } + + new (&myBlock_) Eigen::Block(xpr_.matrix_.block(densei_, densej_, denseRows_, denseCols_)); + } + + template + inline ScalarType coeffInternal(Index row, Index col) const + { + // We leave index checking up to the Block class + if(blockType_ == Plain) + { + return myBlock_(row, col); + } + else if(blockType_ == SelfAdjoint) + { + if(row <= col) + return myBlock_(row, col); + else + return myBlock_.transpose()(row, col); + } + else + { + return myBlock_.transpose()(row, col); + } + } + + template + void _doAssign(const SymmetricBlockMatrixBlockExpr& other) + { + if(blockType_ == SelfAdjoint) + { + assert(other.blockType() == SelfAdjoint); + triangularView() = other.triangularView().nestedExpression(); + } + else if(blockType_ == Plain) + { + assert(other.blockType() == Plain || other.blockType() == Transposed); + if(other.blockType() == Transposed) + myBlock_ = other.myBlock_.transpose(); + else + myBlock_ = other.myBlock_; + } + else + { + assert(other.blockType() == Plain || other.blockType() == Transposed); + if(other.blockType() == Transposed) + myBlock_.transpose() = other.myBlock_.transpose(); + else + myBlock_.transpose() = other.myBlock_; + } + } + + + }; + +} diff --git a/gtsam/base/tests/testSymmetricBlockMatrix.cpp b/gtsam/base/tests/testSymmetricBlockMatrix.cpp new file mode 100644 index 000000000..a4b330000 --- /dev/null +++ b/gtsam/base/tests/testSymmetricBlockMatrix.cpp @@ -0,0 +1,188 @@ +/* ---------------------------------------------------------------------------- + +* 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 testSymmetricBlockMatrix.cpp +* @brief Unit tests for SymmetricBlockMatrix class +* @author Richard Roberts +**/ + +#include +#include +#include + +using namespace std; +using namespace gtsam; +using boost::assign::list_of; + +static SymmetricBlockMatrix testBlockMatrix( + list_of(3)(2)(1), + (Mat(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)); + +/* ************************************************************************* */ +TEST(SymmetricBlockMatrix, ReadBlocks) +{ + // On the diagonal + Matrix expected1 = (Mat(2, 2) << + 22, 23, + 23, 29); + Matrix actual1 = testBlockMatrix(1, 1); + // Test only writing the upper triangle for efficiency + Matrix actual1t = Matrix::Zero(2, 2); + actual1t.triangularView() = testBlockMatrix(1, 1).triangularView(); + EXPECT(assert_equal(expected1, actual1)); + EXPECT(assert_equal(Matrix(expected1.triangularView()), actual1t)); + + // Above the diagonal + Matrix expected2 = (Mat(3, 2) << + 4, 5, + 10, 11, + 16, 17); + Matrix actual2 = testBlockMatrix(0, 1); + EXPECT(assert_equal(expected2, actual2)); + + // Below the diagonal + Matrix expected3 = (Mat(2, 3) << + 4, 10, 16, + 5, 11, 17); + Matrix actual3 = testBlockMatrix(1, 0); + EXPECT(assert_equal(expected3, actual3)); +} + +/* ************************************************************************* */ +TEST(SymmetricBlockMatrix, WriteBlocks) +{ + // On the diagonal + Matrix expected1 = testBlockMatrix(1, 1); + SymmetricBlockMatrix bm1 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix); + bm1(1, 1) = expected1.selfadjointView(); // Verified with debugger that this only writes the upper triangle + Matrix actual1 = bm1(1, 1); + EXPECT(assert_equal(expected1, actual1)); + + // On the diagonal + Matrix expected1p = testBlockMatrix(1, 1); + SymmetricBlockMatrix bm1p = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix); + bm1p(1, 1) = expected1p; // Verified with debugger that this only writes the upper triangle + Matrix actual1p = bm1p(1, 1); + EXPECT(assert_equal(expected1p, actual1p)); + + // Above the diagonal + Matrix expected2 = testBlockMatrix(0, 1); + SymmetricBlockMatrix bm2 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix); + bm2(0, 1) = expected2; + Matrix actual2 = bm2(0, 1); + EXPECT(assert_equal(expected2, actual2)); + + // Below the diagonal + Matrix expected3 = testBlockMatrix(1, 0); + SymmetricBlockMatrix bm3 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix); + bm3(1, 0) = expected3; + Matrix actual3 = bm3(1, 0); + EXPECT(assert_equal(expected3, actual3)); +} + +/* ************************************************************************* */ +TEST(SymmetricBlockMatrix, Ranges) +{ + // On the diagonal + Matrix expected1 = (Mat(3, 3) << + 22, 23, 24, + 23, 29, 30, + 24, 30, 36); + Matrix actual1 = testBlockMatrix.range(1, 3, 1, 3).selfadjointView(); + Matrix actual1a = testBlockMatrix.range(1, 3, 1, 3); + EXPECT(assert_equal(expected1, actual1)); + EXPECT(assert_equal(expected1, actual1a)); + + // Above the diagonal + Matrix expected2 = (Mat(3, 1) << + 24, + 30, + 36); + Matrix actual2 = testBlockMatrix.range(1, 3, 2, 3).knownOffDiagonal(); + Matrix actual2a = testBlockMatrix.range(1, 3, 2, 3); + EXPECT(assert_equal(expected2, actual2)); + EXPECT(assert_equal(expected2, actual2a)); + + // Below the diagonal + Matrix expected3 = (Mat(3, 3) << + 4, 10, 16, + 5, 11, 17, + 6, 12, 18); + Matrix actual3 = testBlockMatrix.range(1, 3, 0, 1).knownOffDiagonal(); + Matrix actual3a = testBlockMatrix.range(1, 3, 0, 1); + EXPECT(assert_equal(expected3, actual3)); + EXPECT(assert_equal(expected3, actual3a)); +} + +/* ************************************************************************* */ +TEST(SymmetricBlockMatrix, expressions) +{ + SymmetricBlockMatrix expected1(list_of(2)(3)(1), (Mat(6, 6) << + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 4, 6, 8, 0, + 0, 0, 0, 9, 12, 0, + 0, 0, 0, 0, 16, 0, + 0, 0, 0, 0, 0, 0)); + + SymmetricBlockMatrix expected2(list_of(2)(3)(1), (Mat(6, 6) << + 0, 0, 10, 15, 20, 0, + 0, 0, 12, 18, 24, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0)); + + Matrix a = (Mat(1, 3) << 2, 3, 4); + Matrix b = (Mat(1, 2) << 5, 6); + + SymmetricBlockMatrix bm1(list_of(2)(3)(1)); + bm1.full().triangularView().setZero(); + bm1(1, 1).selfadjointView().rankUpdate(a.transpose()); + EXPECT(assert_equal(Matrix(expected1.full().selfadjointView()), bm1.full().selfadjointView())); + + SymmetricBlockMatrix bm2(list_of(2)(3)(1)); + bm2.full().triangularView().setZero(); + bm2(0, 1).knownOffDiagonal() += b.transpose() * a; + EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm2.full().selfadjointView())); + + SymmetricBlockMatrix bm3(list_of(2)(3)(1)); + bm3.full().triangularView().setZero(); + bm3(1, 0).knownOffDiagonal() += a.transpose() * b; + EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm3.full().selfadjointView())); + + SymmetricBlockMatrix bm4(list_of(2)(3)(1)); + bm4.full().triangularView().setZero(); + bm4(1, 1) += expected1(1, 1); + EXPECT(assert_equal(Matrix(expected1.full().selfadjointView()), bm4.full().selfadjointView())); + + SymmetricBlockMatrix bm5(list_of(2)(3)(1)); + bm5.full().triangularView().setZero(); + bm5(0, 1) += expected2(0, 1); + EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm5.full().selfadjointView())); + + SymmetricBlockMatrix bm6(list_of(2)(3)(1)); + bm6.full().triangularView().setZero(); + bm6(1, 0) += expected2(1, 0); + EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm6.full().selfadjointView())); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 404bf50b3..67ad682b5 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -129,8 +129,8 @@ HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) : if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); info_(0,0) = Sigma.inverse(); // G - info_(0,1) = info_(0,0) * mu; // g - info_(1,1)(0,0) = mu.dot(info_(0,1).col(0)); // f + info_(0,1) = info_(0,0).selfadjointView() * mu; // g + info_(1,1)(0,0) = mu.dot(info_(0,1).knownOffDiagonal().col(0)); // f } /* ************************************************************************* */ @@ -230,11 +230,11 @@ void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) { if(jf.get_model()->isConstrained()) throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); - info.full().noalias() = jf.matrixObject().full().transpose() * + info.full().triangularView() = jf.matrixObject().full().transpose() * (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() * jf.matrixObject().full(); } else { - info.full().noalias() = jf.matrixObject().full().transpose() * jf.matrixObject().full(); + info.full().triangularView() = jf.matrixObject().full().transpose() * jf.matrixObject().full(); } } } @@ -292,7 +292,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, } dims.back() = 1; info_ = SymmetricBlockMatrix(dims); - info_.full().setZero(); + info_.full().triangularView().setZero(); gttoc(allocate); // Form A' * A @@ -318,7 +318,7 @@ void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) c for(const_iterator key=this->begin(); key!=this->end(); ++key) cout << formatter(*key) << "(" << this->getDim(key) << ") "; cout << "\n"; - gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView()), "Augmented information matrix: "); + gtsam::print(Matrix(info_.full().selfadjointView()), "Augmented information matrix: "); } /* ************************************************************************* */ @@ -328,9 +328,9 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { else { if(!Factor::equals(lf, tol)) return false; - Matrix thisMatrix = this->info_.full().selfadjointView(); + Matrix thisMatrix = this->info_.full().selfadjointView(); thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; - Matrix rhsMatrix = static_cast(lf).info_.full().selfadjointView(); + Matrix rhsMatrix = static_cast(lf).info_.full().selfadjointView(); rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; return equal_with_abs_tol(thisMatrix, rhsMatrix, tol); } @@ -339,13 +339,13 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { /* ************************************************************************* */ Matrix HessianFactor::augmentedInformation() const { - return info_.full().selfadjointView(); + return info_.full().selfadjointView(); } /* ************************************************************************* */ Matrix HessianFactor::information() const { - return info_.range(0, this->size(), 0, this->size()).selfadjointView(); + return info_.range(0, this->size(), 0, this->size()).selfadjointView(); } /* ************************************************************************* */ @@ -369,7 +369,7 @@ double HessianFactor::error(const VectorValues& c) const { // NOTE may not be as efficient const Vector x = c.vector(this->keys()); xtg = x.dot(linearTerm()); - xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; + xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; return 0.5 * (f - 2.0 * xtg + xGx); } @@ -397,12 +397,7 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte DenseIndex slot2 = (j2 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j2]; for(DenseIndex j1=0; j1<=j2; ++j1) { DenseIndex slot1 = (j1 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j1]; - if(slot2 > slot1) - info_(slot1, slot2).noalias() += update.info_(j1, j2); - else if(slot1 > slot2) - info_(slot2, slot1).noalias() += update.info_(j1, j2).transpose(); - else - info_(slot1, slot2).triangularView() += update.info_(j1, j2); + info_(slot1, slot2) += update.info_(j1, j2); } } gttoc(update); @@ -458,48 +453,21 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt for(DenseIndex j1 = 0; j1 <= j2; ++j1) { // Vertical block of Hessian DenseIndex slot1 = (j1 == update.size()) ? nrInfoBlocks-1 : slots[j1]; assert(slot1 >= 0 && slot1 < nrInfoBlocks); - updateBlocks.offset(0); - if(slot2 > slot1) - info_(slot1, slot2).noalias() += updateBlocks(j1).transpose() * updateBlocks(j2); - else if(slot1 > slot2) - info_(slot2, slot1).noalias() += updateBlocks(j2).transpose() * updateBlocks(j1); + if(slot1 != slot2) + info_(slot1, slot2).knownOffDiagonal() += updateBlocks(j1).transpose() * updateBlocks(j2); else - info_(slot1, slot2).triangularView() += updateBlocks(j1).transpose() * updateBlocks(j2); + info_(slot1, slot2).selfadjointView().rankUpdate(updateBlocks(j1).transpose()); } } gttoc(update); } } -/* ************************************************************************* */ -GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFrontals) -{ - gttic(HessianFactor_splitEliminatedFactor); - - // Create one big conditionals with many frontal variables. - gttic(Construct_conditional); - const size_t varDim = info_.offset(nrFrontals); - VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(info_, varDim); - Ab.full() = info_.range(0, nrFrontals, 0, info_.nBlocks()); - GaussianConditional::shared_ptr conditional = boost::make_shared( - keys_, nrFrontals, Ab); - gttoc(Construct_conditional); - - gttic(Remaining_factor); - // Take lower-right block of Ab_ to get the new factor - info_.blockStart() = nrFrontals; - // Assign the keys - keys_.erase(begin(), begin() + nrFrontals); - gttoc(Remaining_factor); - - return conditional; -} - /* ************************************************************************* */ GaussianFactor::shared_ptr HessianFactor::negate() const { shared_ptr result = boost::make_shared(*this); - result->info_.full() = -result->info_.full(); // Negate the information matrix of the result + result->info_.full().triangularView() = -result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result return result; } @@ -521,12 +489,12 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, Vector xj = x.at(keys_[j]); DenseIndex i = 0; for (; i < j; ++i) - y[i] += info_(i, j) * xj; + y[i] += info_(i, j).knownOffDiagonal() * xj; // blocks on the diagonal are only half - y[i] += info_(j, j).selfadjointView() * xj; + y[i] += info_(j, j).selfadjointView() * xj; // for below diagonal, we take transpose block from upper triangular part for (i = j + 1; i < (DenseIndex)size(); ++i) - y[i] += info_(j, i).transpose() * xj; + y[i] += info_(i, j).knownOffDiagonal() * xj; } // copy to yvalues @@ -546,7 +514,7 @@ VectorValues HessianFactor::gradientAtZero() const { VectorValues g; size_t n = size(); for (size_t j = 0; j < n; ++j) - g.insert(keys_[j],-info_(j,n)); + g.insert(keys_[j], -info_(j,n).knownOffDiagonal()); return g; } @@ -567,12 +535,17 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) } // Do dense elimination - if(!choleskyPartial(jointFactor->info_.matrix(), jointFactor->info_.offset(keys.size()))) + GaussianConditional::shared_ptr conditional; + try { + VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(keys.size()); + conditional = boost::make_shared(jointFactor->keys(), keys.size(), Ab); + // Erase the eliminated keys in the remaining factor + jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + keys.size()); + } catch(CholeskyFailed&) { throw IndeterminantLinearSystemException(keys.front()); + } - // Split conditional - GaussianConditional::shared_ptr conditional = jointFactor->splitEliminatedFactor(keys.size()); - + // Return result return make_pair(conditional, jointFactor); } diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 1467d5783..a3a38e4dc 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -265,13 +265,13 @@ namespace gtsam { * as described above. See HessianFactor class documentation above to explain that only the * upper-triangular part of the information matrix is stored and returned by this function. */ - constBlock info() const { return info_.full(); } + SymmetricBlockMatrix::constBlock info() const { return info_.full(); } /** Return the upper-triangular part of the full *augmented* information matrix, * as described above. See HessianFactor class documentation above to explain that only the * upper-triangular part of the information matrix is stored and returned by this function. */ - Block info() { return info_.full(); } + SymmetricBlockMatrix::Block info() { return info_.full(); } /** Return the constant term \f$ f \f$ as described above * @return The constant term \f$ f \f$ @@ -287,21 +287,25 @@ namespace gtsam { * @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$ */ - constBlock::ColXpr linearTerm(const_iterator j) const { return info_(j-begin(), size()).col(0); } + constBlock::OffDiagonal::ColXpr linearTerm(const_iterator j) const { + return info_(j-begin(), size()).knownOffDiagonal().col(0); } /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. * @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$ */ - Block::ColXpr linearTerm(iterator j) { return info_(j-begin(), size()).col(0); } + Block::OffDiagonal::ColXpr linearTerm(iterator j) { + return info_(j-begin(), size()).knownOffDiagonal().col(0); } /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ - constBlock::ColXpr linearTerm() const { return info_.range(0, this->size(), this->size(), this->size() + 1).col(0); } + constBlock::OffDiagonal::ColXpr linearTerm() const { + return info_.range(0, this->size(), this->size(), this->size() + 1).knownOffDiagonal().col(0); } /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ - Block::ColXpr linearTerm() { return info_.range(0, this->size(), this->size(), this->size() + 1).col(0); } + Block::OffDiagonal::ColXpr linearTerm() { + return info_.range(0, this->size(), this->size(), this->size() + 1).knownOffDiagonal().col(0); } /** Return the augmented information matrix represented by this GaussianFactor. * The augmented information matrix contains the information matrix with an @@ -401,9 +405,6 @@ namespace gtsam { private: - /** split partially eliminated factor */ - boost::shared_ptr splitEliminatedFactor(size_t nrFrontals); - /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/tests/testHessianFactorUnordered.cpp b/gtsam/linear/tests/testHessianFactorUnordered.cpp index bad2b14c4..5e6f49235 100644 --- a/gtsam/linear/tests/testHessianFactorUnordered.cpp +++ b/gtsam/linear/tests/testHessianFactorUnordered.cpp @@ -86,7 +86,7 @@ TEST(HessianFactor, ConversionConstructor) /* ************************************************************************* */ TEST(HessianFactor, Constructor1) { - Matrix G = (Mat(2,2) << 3.0, 5.0, 0.0, 6.0); + Matrix G = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0); Vector g = (Vec(2) << -8.0, -9.0); double f = 10.0; HessianFactor factor(0, G, g, f); @@ -132,7 +132,7 @@ TEST(HessianFactor, Constructor2) { Matrix G11 = (Mat(1,1) << 1.0); Matrix G12 = (Mat(1,2) << 2.0, 4.0); - Matrix G22 = (Mat(2,2) << 3.0, 5.0, 0.0, 6.0); + Matrix G22 = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0); Vector g1 = (Vec(1) << -7.0); Vector g2 = (Vec(2) << -8.0, -9.0); double f = 10.0; @@ -175,10 +175,10 @@ TEST(HessianFactor, Constructor3) Matrix G12 = (Mat(1,2) << 2.0, 4.0); Matrix G13 = (Mat(1,3) << 3.0, 6.0, 9.0); - Matrix G22 = (Mat(2,2) << 3.0, 5.0, 0.0, 6.0); + Matrix G22 = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0); Matrix G23 = (Mat(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); - Matrix G33 = (Mat(3,3) << 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); + Matrix G33 = (Mat(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0); Vector g1 = (Vec(1) << -7.0); Vector g2 = (Vec(2) << -8.0, -9.0); @@ -222,10 +222,10 @@ TEST(HessianFactor, ConstructorNWay) Matrix G12 = (Mat(1,2) << 2.0, 4.0); Matrix G13 = (Mat(1,3) << 3.0, 6.0, 9.0); - Matrix G22 = (Mat(2,2) << 3.0, 5.0, 0.0, 6.0); + Matrix G22 = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0); Matrix G23 = (Mat(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); - Matrix G33 = (Mat(3,3) << 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); + Matrix G33 = (Mat(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0); Vector g1 = (Vec(1) << -7.0); Vector g2 = (Vec(2) << -8.0, -9.0); @@ -423,8 +423,7 @@ TEST(HessianFactor, combine) { -100.0000, 0.0, 20.0000, 0.0, 80.0000, 0.0, -20.0000, 0.0, -100.0000, 0.0, 20.0000, 0.0, 80.0000, 14.0000, 25.0000, -17.5000, -5.0000, 3.5000, -20.0000, 14.0000, 7.4500); - EXPECT(assert_equal(Matrix(expected.triangularView()), - Matrix(actual.matrixObject().full().triangularView()), tol)); + EXPECT(assert_equal(expected, Matrix(actual.matrixObject().full()), tol)); } diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 8cae3d3d3..f26001b16 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -123,10 +123,9 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con jacFactor->getb() = -jacFactor->unweighted_error(delta); } else { HessianFactor::shared_ptr hesFactor = boost::dynamic_pointer_cast(linFactor); - size_t dim = hesFactor->linearTerm().size(); - Eigen::Block Gview = hesFactor->info().block(0, 0, dim, dim); + SymmetricBlockMatrix::constBlock Gview = hesFactor->matrixObject().range(0, hesFactor->size(), 0, hesFactor->size()); Vector deltaVector = delta.vector(keys()); - Vector G_delta = Gview.selfadjointView() * deltaVector; + Vector G_delta = Gview.selfadjointView() * deltaVector; hesFactor->constantTerm() += deltaVector.dot(G_delta) - 2.0 * deltaVector.dot(hesFactor->linearTerm()); hesFactor->linearTerm() -= G_delta; } diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 7b7738250..0dca18a1f 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -76,7 +76,9 @@ Matrix Marginals::marginalInformation(Key variable) const { /* ************************************************************************* */ JointMarginal Marginals::jointMarginalCovariance(const std::vector& variables) const { JointMarginal info = jointMarginalInformation(variables); - info.blockMatrix_.full() = info.blockMatrix_.full().inverse(); + info.blockMatrix_.full().triangularView() = + info.blockMatrix_.full().selfadjointView().llt().solve( + Matrix::Identity(info.blockMatrix_.full().rows(), info.blockMatrix_.full().rows())).triangularView(); return info; } diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index fbcfcf15b..dd585e902 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -116,7 +116,7 @@ public: * in scope while this view is needed. Otherwise assign this block object to a Matrix * to store it. */ - const Matrix& fullMatrix() const { return blockMatrix_.matrix(); } + Eigen::SelfAdjointView fullMatrix() const { return blockMatrix_.matrix(); } /** Print */ void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 1a455001b..5d73bcab8 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -69,7 +69,7 @@ TEST( AntiFactor, NegativeHessian) size_t variable_count = originalFactor->size(); for(size_t i = 0; i < variable_count; ++i){ for(size_t j = i; j < variable_count; ++j){ - Matrix expected_G = -originalHessian->info(originalHessian->begin()+i, originalHessian->begin()+j); + Matrix expected_G = -Matrix(originalHessian->info(originalHessian->begin()+i, originalHessian->begin()+j)); Matrix actual_G = antiHessian->info(antiHessian->begin()+i, antiHessian->begin()+j); CHECK(assert_equal(expected_G, actual_G, 1e-5)); } diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index 526674a04..f8dca75a4 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -153,7 +153,7 @@ LinearizedHessianFactor::LinearizedHessianFactor( // Update the BlockInfo accessor info_ = SymmetricBlockMatrix(dims, dims+hessian->size()+1); // Copy the augmented matrix holding G, g, and f - info_.matrix() = hessian->info(); + info_.full() = hessian->info(); } /* ************************************************************************* */ @@ -166,7 +166,7 @@ void LinearizedHessianFactor::print(const std::string& s, const KeyFormatter& ke std::cout << keyFormatter(key) << " "; std::cout << std::endl; - gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView()), "Ab^T * Ab: "); + gtsam::print(Matrix(info_.full()), "Ab^T * Ab: "); lin_points_.print("Linearization Point: "); } @@ -177,9 +177,9 @@ bool LinearizedHessianFactor::equals(const NonlinearFactor& expected, double tol const This *e = dynamic_cast (&expected); if (e) { - Matrix thisMatrix = this->info_.full().selfadjointView(); + Matrix thisMatrix = this->info_.full(); thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; - Matrix rhsMatrix = e->info_.full().selfadjointView(); + Matrix rhsMatrix = e->info_.full(); rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; return Base::equals(expected, tol) @@ -207,7 +207,7 @@ double LinearizedHessianFactor::error(const Values& c) const { // error 0.5*(f - 2*x'*g + x'*G*x) double f = constantTerm(); double xtg = dx.dot(linearTerm()); - double xGx = dx.transpose() * squaredTerm().selfadjointView() * dx; + double xGx = dx.transpose() * squaredTerm() * dx; return 0.5 * (f - 2.0 * xtg + xGx); } @@ -229,11 +229,11 @@ LinearizedHessianFactor::linearize(const Values& c) const { // f2 = f1 - 2*dx'*g1 + dx'*G1*dx //newInfo(this->size(), this->size())(0,0) += -2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView() * dx; - double f = constantTerm() - 2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView() * dx; + double f = constantTerm() - 2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm() * dx; // g2 = g1 - G1*dx //newInfo.rangeColumn(0, this->size(), this->size(), 0) -= squaredTerm().selfadjointView() * dx; - Vector g = linearTerm() - squaredTerm().selfadjointView() * dx; + Vector g = linearTerm() - squaredTerm() * dx; std::vector gs; for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) { gs.push_back(g.segment(info_.offset(i), info_.offset(i+1) - info_.offset(i))); diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 9e2319ae6..7be219187 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -176,8 +176,8 @@ public: /** hessian block data types */ 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) + typedef SymmetricBlockMatrix::Block::OffDiagonal::ColXpr Column; ///< A column containing the linear term h + typedef SymmetricBlockMatrix::constBlock::OffDiagonal::ColXpr constColumn; ///< A column containing the linear term h (const version) protected: @@ -220,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_(j - this->begin(), this->size()).col(0); } + constColumn linearTerm(const_iterator j) const { return info_(j - this->begin(), this->size()).knownOffDiagonal().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_.range(0, this->size(), this->size(), this->size() + 1).col(0); }; + constColumn linearTerm() const { return info_.range(0, this->size(), this->size(), this->size() + 1).knownOffDiagonal().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 @@ -242,7 +242,7 @@ public: * See HessianFactor class documentation above to explain that only the * upper-triangular part of the information matrix is stored and returned by this function. */ - constBlock squaredTerm() const { return info_.range(0, this->size(), 0, this->size()); } + constBlock::SelfAdjointView squaredTerm() const { return info_.range(0, this->size(), 0, this->size()).selfadjointView(); } /** get the dimension of the factor (number of rows on linearization) */