Removed old VerticalBlockView and SymmetricBlockView (replaced with VerticalBlockMatrix and SymmetricBlockMatrix)
Richard Roberts
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