Removed old VerticalBlockView and SymmetricBlockView (replaced with VerticalBlockMatrix and SymmetricBlockMatrix)
							parent
							
								
									0b58b0166a
								
							
						
					
					
						commit
						f1c9322d37
					
				|  | @ -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 <gtsam/base/Matrix.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| template<class MATRIX> 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 MATRIX> | ||||
| class VerticalBlockView { | ||||
| public: | ||||
|   typedef MATRIX FullMatrix; | ||||
|   typedef Eigen::Block<MATRIX> Block; | ||||
|   typedef Eigen::Block<const MATRIX> constBlock; | ||||
| 
 | ||||
|   // columns of blocks
 | ||||
|   typedef Eigen::VectorBlock<typename MATRIX::ColXpr> Column; | ||||
|   typedef Eigen::VectorBlock<const typename MATRIX::ConstColXpr> constColumn; | ||||
| 
 | ||||
| protected: | ||||
|   FullMatrix& matrix_; // the reference to the full matrix
 | ||||
|   std::vector<size_t> 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<class RHS> | ||||
|   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<typename ITERATOR> | ||||
|   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<typename ITERATOR> | ||||
|   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<class RHS> | ||||
|   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<size_t>::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<class RHS> | ||||
|   VerticalBlockView<MATRIX>& 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<MATRIX>& 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<typename ITERATOR> | ||||
|   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<class OTHER> friend class SymmetricBlockView; | ||||
|   template<class RELATED> friend class VerticalBlockView; | ||||
| 
 | ||||
| private: | ||||
|   /** Serialization function */ | ||||
|   friend class boost::serialization::access; | ||||
|   template<class ARCHIVE> | ||||
|   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 MATRIX> | ||||
| class SymmetricBlockView { | ||||
| public: | ||||
|   typedef MATRIX FullMatrix; | ||||
|   typedef Eigen::Block<MATRIX> Block; | ||||
|   typedef Eigen::Block<const MATRIX> 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<size_t> 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<typename ITERATOR> | ||||
|   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<typename ITERATOR> | ||||
|   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<class RHS> | ||||
|   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<size_t>::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<class RHSMATRIX> | ||||
|   SymmetricBlockView<MATRIX>& assignNoalias(const SymmetricBlockView<RHSMATRIX>& 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<MATRIX>& 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<typename ITERATOR> | ||||
|   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<class RELATED> friend class SymmetricBlockView; | ||||
|   template<class OTHER> friend class VerticalBlockView; | ||||
| 
 | ||||
| private: | ||||
|   /** Serialization function */ | ||||
|   friend class boost::serialization::access; | ||||
|   template<class ARCHIVE> | ||||
|   void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
|     ar & BOOST_SERIALIZATION_NVP(matrix_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(blockStart_); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
| } | ||||
|  | @ -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 <iostream> | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/base/blockMatrices.h> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(testBlockMatrices, jacobian_factor1) { | ||||
|   typedef Matrix AbMatrix; | ||||
|   typedef VerticalBlockView<AbMatrix> 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<AbMatrix> 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<InfoMatrix> 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); } | ||||
| /* ************************************************************************* */ | ||||
|  | @ -18,7 +18,6 @@ | |||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/base/blockMatrices.h> | ||||
| #include <gtsam/linear/GaussianBayesTree.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/nonlinear/Values.h> | ||||
|  |  | |||
|  | @ -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<GaussianFactor> | ||||
| LinearizedHessianFactor::linearize(const Values& c) const { | ||||
| 
 | ||||
|   // Make a copy of the info matrix
 | ||||
|   Matrix newMatrix; | ||||
|   SymmetricBlockView<Matrix> 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<Eigen::Upper>() * dx;
 | ||||
|   Vector g = linearTerm() - squaredTerm().selfadjointView<Eigen::Upper>() * dx; | ||||
|   std::vector<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<Matrix> 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)); | ||||
|     } | ||||
|   } | ||||
|  |  | |||
|  | @ -22,7 +22,6 @@ | |||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| #include <gtsam/linear/JacobianFactor.h> | ||||
| #include <gtsam/linear/HessianFactor.h> | ||||
| #include <gtsam/base/blockMatrices.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|  | @ -85,12 +84,10 @@ public: | |||
|   /** shared pointer for convenience */ | ||||
|   typedef boost::shared_ptr<LinearizedJacobianFactor> shared_ptr; | ||||
| 
 | ||||
|   typedef Matrix AbMatrix; | ||||
|   typedef VerticalBlockView<AbMatrix> 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<Base>(*this)); | ||||
|     ar & BOOST_SERIALIZATION_NVP(matrix_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(Ab_); | ||||
|   } | ||||
| }; | ||||
|  | @ -179,17 +174,15 @@ public: | |||
|   typedef boost::shared_ptr<LinearizedHessianFactor> shared_ptr; | ||||
| 
 | ||||
|   /** hessian block data types */ | ||||
|   typedef Matrix InfoMatrix; ///< The full augmented Hessian
 | ||||
|   typedef SymmetricBlockView<InfoMatrix> 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 <emph>upper-triangular part</emph> 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<Base>(*this)); | ||||
|     ar & BOOST_SERIALIZATION_NVP(matrix_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(info_); | ||||
|   } | ||||
| }; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue