Merge branch 'origin/feature/LM/SymmetricBlockMatrix'
dellaert
commit
ba6f8b7cca
16
.cproject
16
.cproject
|
|
@ -2092,6 +2092,22 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSymmetricBlockMatrix.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testSymmetricBlockMatrix.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testVerticalBlockMatrix.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testVerticalBlockMatrix.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
|
|||
|
|
@ -1,20 +1,20 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SymmetricBlockMatrix.cpp
|
||||
* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional.
|
||||
* @author Richard Roberts
|
||||
* @date Sep 18, 2010
|
||||
*/
|
||||
* @file SymmetricBlockMatrix.cpp
|
||||
* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional.
|
||||
* @author Richard Roberts
|
||||
* @date Sep 18, 2010
|
||||
*/
|
||||
|
||||
#include <gtsam/base/SymmetricBlockMatrix.h>
|
||||
#include <gtsam/base/VerticalBlockMatrix.h>
|
||||
|
|
@ -23,56 +23,60 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(const SymmetricBlockMatrix& other)
|
||||
{
|
||||
SymmetricBlockMatrix result;
|
||||
result.variableColOffsets_.resize(other.nBlocks() + 1);
|
||||
for(size_t i = 0; i < result.variableColOffsets_.size(); ++i)
|
||||
result.variableColOffsets_[i] =
|
||||
other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_];
|
||||
result.matrix_.resize(other.cols(), other.cols());
|
||||
result.assertInvariants();
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& other)
|
||||
{
|
||||
SymmetricBlockMatrix result;
|
||||
result.variableColOffsets_.resize(other.nBlocks() + 1);
|
||||
for(size_t i = 0; i < result.variableColOffsets_.size(); ++i)
|
||||
result.variableColOffsets_[i] =
|
||||
other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_];
|
||||
result.matrix_.resize(other.cols(), other.cols());
|
||||
result.assertInvariants();
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial(DenseIndex nFrontals)
|
||||
{
|
||||
// Do dense elimination
|
||||
if(!blockStart() == 0)
|
||||
throw std::invalid_argument("Can only do Cholesky when the SymmetricBlockMatrix is not a restricted view, i.e. when blockStart == 0.");
|
||||
if(!gtsam::choleskyPartial(matrix_, offset(nFrontals)))
|
||||
throw CholeskyFailed();
|
||||
|
||||
// Split conditional
|
||||
|
||||
// Create one big conditionals with many frontal variables.
|
||||
gttic(Construct_eliminated);
|
||||
const size_t varDim = offset(nFrontals);
|
||||
VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim);
|
||||
Ab.full() = matrix_.topRows(varDim);
|
||||
Ab.full().triangularView<Eigen::StrictlyLower>().setZero();
|
||||
gttoc(Construct_conditional);
|
||||
|
||||
gttic(Remaining_factor);
|
||||
// Take lower-right block of Ab_ to get the remaining factor
|
||||
blockStart() = nFrontals;
|
||||
gttoc(Remaining_factor);
|
||||
|
||||
return Ab;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(
|
||||
const SymmetricBlockMatrix& other) {
|
||||
SymmetricBlockMatrix result;
|
||||
result.variableColOffsets_.resize(other.nBlocks() + 1);
|
||||
for (size_t i = 0; i < result.variableColOffsets_.size(); ++i)
|
||||
result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_
|
||||
+ i] - other.variableColOffsets_[other.blockStart_];
|
||||
result.matrix_.resize(other.cols(), other.cols());
|
||||
result.assertInvariants();
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(
|
||||
const VerticalBlockMatrix& other) {
|
||||
SymmetricBlockMatrix result;
|
||||
result.variableColOffsets_.resize(other.nBlocks() + 1);
|
||||
for (size_t i = 0; i < result.variableColOffsets_.size(); ++i)
|
||||
result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_
|
||||
+ i] - other.variableColOffsets_[other.blockStart_];
|
||||
result.matrix_.resize(other.cols(), other.cols());
|
||||
result.assertInvariants();
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial(
|
||||
DenseIndex nFrontals) {
|
||||
// Do dense elimination
|
||||
if (!blockStart() == 0)
|
||||
throw std::invalid_argument(
|
||||
"Can only do Cholesky when the SymmetricBlockMatrix is not a restricted view, i.e. when blockStart == 0.");
|
||||
if (!gtsam::choleskyPartial(matrix_, offset(nFrontals)))
|
||||
throw CholeskyFailed();
|
||||
|
||||
// Split conditional
|
||||
|
||||
// Create one big conditionals with many frontal variables.
|
||||
gttic(Construct_eliminated);
|
||||
const size_t varDim = offset(nFrontals);
|
||||
VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim);
|
||||
Ab.full() = matrix_.topRows(varDim);
|
||||
Ab.full().triangularView<Eigen::StrictlyLower>().setZero();
|
||||
gttoc(Construct_conditional);
|
||||
|
||||
gttic(Remaining_factor);
|
||||
// Take lower-right block of Ab_ to get the remaining factor
|
||||
blockStart() = nFrontals;
|
||||
gttoc(Remaining_factor);
|
||||
|
||||
return Ab;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
} //\ namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -1,20 +1,20 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SymmetricBlockMatrix.h
|
||||
* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional.
|
||||
* @author Richard Roberts
|
||||
* @date Sep 18, 2010
|
||||
*/
|
||||
* @file SymmetricBlockMatrix.h
|
||||
* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional.
|
||||
* @author Richard Roberts
|
||||
* @date Sep 18, 2010
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
|
@ -23,296 +23,352 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
class VerticalBlockMatrix;
|
||||
// Forward declarations
|
||||
class VerticalBlockMatrix;
|
||||
|
||||
/**
|
||||
* This class stores a dense matrix and allows it to be accessed as a
|
||||
* collection of blocks. When constructed, the caller must provide the
|
||||
* dimensions of the blocks.
|
||||
*
|
||||
* The block structure is symmetric, but the underlying matrix does not
|
||||
* necessarily need to be.
|
||||
*
|
||||
* This class also has a parameter that can be changed after construction to
|
||||
* change the apparent matrix view. firstBlock() determines the block that
|
||||
* appears to have index 0 for all operations (except re-setting firstBlock).
|
||||
*
|
||||
* @addtogroup base */
|
||||
class GTSAM_EXPORT SymmetricBlockMatrix {
|
||||
public:
|
||||
typedef SymmetricBlockMatrix This;
|
||||
typedef SymmetricBlockMatrixBlockExpr<This> Block;
|
||||
typedef SymmetricBlockMatrixBlockExpr<const This> constBlock;
|
||||
|
||||
protected:
|
||||
|
||||
Matrix matrix_; ///< The full matrix
|
||||
|
||||
/// the starting columns of each block (0-based)
|
||||
FastVector<DenseIndex> variableColOffsets_;
|
||||
|
||||
/// Changes apparent matrix view, see main class comment.
|
||||
DenseIndex blockStart_;
|
||||
|
||||
public:
|
||||
/// Construct from an empty matrix (asserts that the matrix is empty)
|
||||
SymmetricBlockMatrix() :
|
||||
blockStart_(0) {
|
||||
variableColOffsets_.push_back(0);
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/// Construct from a container of the sizes of each block.
|
||||
template<typename CONTAINER>
|
||||
SymmetricBlockMatrix(const CONTAINER& dimensions) :
|
||||
blockStart_(0) {
|
||||
fillOffsets(dimensions.begin(), dimensions.end());
|
||||
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/// Construct from iterator over the sizes of each vertical block.
|
||||
template<typename ITERATOR>
|
||||
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim) :
|
||||
blockStart_(0) {
|
||||
fillOffsets(firstBlockDim, lastBlockDim);
|
||||
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/**
|
||||
* This class stores a dense matrix and allows it to be accessed as a collection of blocks. When
|
||||
* constructed, the caller must provide the dimensions of the blocks.
|
||||
*
|
||||
* The block structure is symmetric, but the underlying matrix does not necessarily need to be.
|
||||
*
|
||||
* This class also has a parameter that can be changed after construction to change the apparent
|
||||
* matrix view. firstBlock() determines the block that appears to have index 0 for all operations
|
||||
* (except re-setting firstBlock()).
|
||||
*
|
||||
* @addtogroup base */
|
||||
class GTSAM_EXPORT SymmetricBlockMatrix
|
||||
{
|
||||
public:
|
||||
typedef SymmetricBlockMatrix This;
|
||||
typedef SymmetricBlockMatrixBlockExpr<This> Block;
|
||||
typedef SymmetricBlockMatrixBlockExpr<const This> constBlock;
|
||||
* @brief Construct from a container of the sizes of each vertical block
|
||||
* and a pre-prepared matrix.
|
||||
*/
|
||||
template<typename CONTAINER>
|
||||
SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) :
|
||||
blockStart_(0) {
|
||||
matrix_.resize(matrix.rows(), matrix.cols());
|
||||
matrix_.triangularView<Eigen::Upper>() =
|
||||
matrix.triangularView<Eigen::Upper>();
|
||||
fillOffsets(dimensions.begin(), dimensions.end());
|
||||
if (matrix_.rows() != matrix_.cols())
|
||||
throw std::invalid_argument("Requested to create a SymmetricBlockMatrix"
|
||||
" from a non-square matrix.");
|
||||
if (variableColOffsets_.back() != matrix_.cols())
|
||||
throw std::invalid_argument(
|
||||
"Requested to create a SymmetricBlockMatrix with dimensions "
|
||||
"that do not sum to the total size of the provided matrix.");
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
protected:
|
||||
Matrix matrix_; ///< The full matrix
|
||||
FastVector<DenseIndex> variableColOffsets_; ///< the starting columns of each block (0-based)
|
||||
/**
|
||||
* Copy the block structure, but do not copy the matrix data. If blockStart()
|
||||
* has been modified, this copies the structure of the corresponding matrix.
|
||||
* In the destination SymmetricBlockMatrix, blockStart() will be 0.
|
||||
*/
|
||||
static SymmetricBlockMatrix LikeActiveViewOf(
|
||||
const SymmetricBlockMatrix& other);
|
||||
|
||||
DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment.
|
||||
/**
|
||||
* Copy the block structure, but do not copy the matrix data. If blockStart()
|
||||
* has been modified, this copies the structure of the corresponding matrix.
|
||||
* In the destination SymmetricBlockMatrix, blockStart() will be 0.
|
||||
*/
|
||||
static SymmetricBlockMatrix LikeActiveViewOf(
|
||||
const VerticalBlockMatrix& other);
|
||||
|
||||
public:
|
||||
/// Construct from an empty matrix (asserts that the matrix is empty)
|
||||
SymmetricBlockMatrix() :
|
||||
blockStart_(0)
|
||||
{
|
||||
variableColOffsets_.push_back(0);
|
||||
assertInvariants();
|
||||
/// Row size
|
||||
DenseIndex rows() const {
|
||||
assertInvariants();
|
||||
return variableColOffsets_.back() - variableColOffsets_[blockStart_];
|
||||
}
|
||||
|
||||
/// Column size
|
||||
DenseIndex cols() const {
|
||||
return rows();
|
||||
}
|
||||
|
||||
/// Block count
|
||||
DenseIndex nBlocks() const {
|
||||
assertInvariants();
|
||||
return variableColOffsets_.size() - 1 - blockStart_;
|
||||
}
|
||||
|
||||
/**
|
||||
* Access the block with vertical block index \c i_block and horizontal block
|
||||
* index \c j_block. Note that the actual block accessed in the underlying
|
||||
* matrix is relative to blockStart().
|
||||
*/
|
||||
Block operator()(DenseIndex i_block, DenseIndex j_block) {
|
||||
return Block(*this, i_block, j_block);
|
||||
}
|
||||
|
||||
/**
|
||||
* Access the block with vertical block index \c i_block and horizontal block
|
||||
* index \c j_block. Note that the actual block accessed in the underlying
|
||||
* matrix is relative to blockStart().
|
||||
*/
|
||||
constBlock operator()(DenseIndex i_block, DenseIndex j_block) const {
|
||||
return constBlock(*this, i_block, j_block);
|
||||
}
|
||||
|
||||
/**
|
||||
* Access the range of blocks starting with vertical block index
|
||||
* \c i_startBlock, ending with vertical block index \c i_endBlock, starting
|
||||
* with horizontal block index \c j_startBlock, and ending with horizontal
|
||||
* block index \c j_endBlock. End block indices are exclusive. Note that the
|
||||
* actual blocks accessed in the underlying matrix are relative to blockStart().
|
||||
*/
|
||||
Block range(DenseIndex i_startBlock, DenseIndex i_endBlock,
|
||||
DenseIndex j_startBlock, DenseIndex j_endBlock) {
|
||||
assertInvariants();
|
||||
return Block(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock,
|
||||
j_endBlock - j_startBlock);
|
||||
}
|
||||
|
||||
/**
|
||||
* Access the range of blocks starting with vertical block index
|
||||
* \c i_startBlock, ending with vertical block index \c i_endBlock, starting
|
||||
* with horizontal block index \c j_startBlock, and ending with horizontal
|
||||
* block index \c j_endBlock. End block indices are exclusive. Note that the
|
||||
* actual blocks accessed in the underlying matrix are relative to blockStart().
|
||||
*/
|
||||
constBlock range(DenseIndex i_startBlock, DenseIndex i_endBlock,
|
||||
DenseIndex j_startBlock, DenseIndex j_endBlock) const {
|
||||
assertInvariants();
|
||||
return constBlock(*this, i_startBlock, j_startBlock,
|
||||
i_endBlock - i_startBlock, j_endBlock - j_startBlock);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the full matrix, *not* including any portions excluded by
|
||||
* firstBlock().
|
||||
*/
|
||||
Block full() {
|
||||
return Block(*this, 0, nBlocks(), 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the full matrix, *not* including any portions excluded by
|
||||
* firstBlock().
|
||||
*/
|
||||
constBlock full() const {
|
||||
return constBlock(*this, 0, nBlocks(), 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Access to full matrix, including any portions excluded by firstBlock()
|
||||
* to other operations.
|
||||
*/
|
||||
Eigen::SelfAdjointView<const Matrix, Eigen::Upper> matrix() const {
|
||||
return matrix_;
|
||||
}
|
||||
|
||||
/** Access to full matrix, including any portions excluded by firstBlock()
|
||||
* to other operations.
|
||||
*/
|
||||
Eigen::SelfAdjointView<Matrix, Eigen::Upper> matrix() {
|
||||
return matrix_;
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the absolute offset in the underlying matrix
|
||||
* of the start of the specified \c block.
|
||||
*/
|
||||
DenseIndex offset(DenseIndex block) const {
|
||||
assertInvariants();
|
||||
DenseIndex actualBlock = block + blockStart_;
|
||||
checkBlock(actualBlock);
|
||||
return variableColOffsets_[actualBlock];
|
||||
}
|
||||
|
||||
/**
|
||||
* Retrieve or modify the first logical block, i.e. the block referenced by
|
||||
* block index 0. Blocks before it will be inaccessible, except by accessing
|
||||
* the underlying matrix using matrix().
|
||||
*/
|
||||
DenseIndex& blockStart() {
|
||||
return blockStart_;
|
||||
}
|
||||
|
||||
/**
|
||||
* Retrieve the first logical block, i.e. the block referenced by block index 0.
|
||||
* Blocks before it will be inaccessible, except by accessing the underlying
|
||||
* matrix using matrix().
|
||||
*/
|
||||
DenseIndex blockStart() const {
|
||||
return blockStart_;
|
||||
}
|
||||
|
||||
/**
|
||||
* Do partial Cholesky in-place and return the eliminated block matrix,
|
||||
* leaving the remaining symmetric matrix in place.
|
||||
*/
|
||||
VerticalBlockMatrix choleskyPartial(DenseIndex nFrontals);
|
||||
|
||||
protected:
|
||||
void assertInvariants() const {
|
||||
assert(matrix_.rows() == matrix_.cols());
|
||||
assert(matrix_.cols() == variableColOffsets_.back());
|
||||
assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
|
||||
}
|
||||
|
||||
void checkBlock(DenseIndex block) const {
|
||||
assert(matrix_.rows() == matrix_.cols());
|
||||
assert(matrix_.cols() == variableColOffsets_.back());
|
||||
assert(block >= 0);
|
||||
assert(block < (DenseIndex)variableColOffsets_.size()-1);
|
||||
assert(
|
||||
variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
|
||||
}
|
||||
|
||||
DenseIndex offsetUnchecked(DenseIndex block) const {
|
||||
return variableColOffsets_[block + blockStart_];
|
||||
}
|
||||
|
||||
//void checkRange(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
|
||||
//{
|
||||
// const DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
|
||||
// const DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
|
||||
// const DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
|
||||
// const DenseIndex j_actualEndBlock = j_endBlock + blockStart_;
|
||||
// checkBlock(i_actualStartBlock);
|
||||
// checkBlock(j_actualStartBlock);
|
||||
// if(i_startBlock != 0 || i_endBlock != 0) {
|
||||
// checkBlock(i_actualStartBlock);
|
||||
// assert(i_actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
// }
|
||||
// if(j_startBlock != 0 || j_endBlock != 0) {
|
||||
// checkBlock(j_actualStartBlock);
|
||||
// assert(j_actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
// }
|
||||
//}
|
||||
|
||||
//void checkRange(DenseIndex startBlock, DenseIndex endBlock) const
|
||||
//{
|
||||
// const DenseIndex actualStartBlock = startBlock + blockStart_;
|
||||
// const DenseIndex actualEndBlock = endBlock + blockStart_;
|
||||
// checkBlock(actualStartBlock);
|
||||
// if(startBlock != 0 || endBlock != 0) {
|
||||
// checkBlock(actualStartBlock);
|
||||
// assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
// }
|
||||
//}
|
||||
|
||||
//Block rangeUnchecked(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock)
|
||||
//{
|
||||
// const DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
|
||||
// const DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
|
||||
// const DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
|
||||
// const DenseIndex j_actualEndBlock = j_endBlock + blockStart_;
|
||||
|
||||
// return Block(matrix(),
|
||||
// variableColOffsets_[i_actualStartBlock],
|
||||
// variableColOffsets_[j_actualStartBlock],
|
||||
// variableColOffsets_[i_actualEndBlock] - variableColOffsets_[i_actualStartBlock],
|
||||
// variableColOffsets_[j_actualEndBlock] - variableColOffsets_[j_actualStartBlock]);
|
||||
//}
|
||||
|
||||
//constBlock rangeUnchecked(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
|
||||
//{
|
||||
// // Convert Block to constBlock
|
||||
// const Block block = const_cast<This*>(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*>(this)->rangeUnchecked(startBlock, endBlock);
|
||||
// return constBlock(matrix(), block.startRow(), block.startCol(), block.rows(), block.cols());
|
||||
//}
|
||||
|
||||
template<typename ITERATOR>
|
||||
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) {
|
||||
variableColOffsets_.resize((lastBlockDim - firstBlockDim) + 1);
|
||||
variableColOffsets_[0] = 0;
|
||||
DenseIndex j = 0;
|
||||
for (ITERATOR dim = firstBlockDim; dim != lastBlockDim; ++dim) {
|
||||
variableColOffsets_[j + 1] = variableColOffsets_[j] + *dim;
|
||||
++j;
|
||||
}
|
||||
}
|
||||
|
||||
/// Construct from a container of the sizes of each block.
|
||||
template<typename CONTAINER>
|
||||
SymmetricBlockMatrix(const CONTAINER& dimensions) :
|
||||
blockStart_(0)
|
||||
{
|
||||
fillOffsets(dimensions.begin(), dimensions.end());
|
||||
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
}
|
||||
friend class VerticalBlockMatrix;
|
||||
template<typename SymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr;
|
||||
|
||||
/// Construct from iterator over the sizes of each vertical block.
|
||||
template<typename ITERATOR>
|
||||
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim) :
|
||||
blockStart_(0)
|
||||
{
|
||||
fillOffsets(firstBlockDim, lastBlockDim);
|
||||
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
}
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<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_);
|
||||
}
|
||||
};
|
||||
|
||||
/// Construct from a container of the sizes of each vertical block and a pre-prepared matrix.
|
||||
template<typename CONTAINER>
|
||||
SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) :
|
||||
blockStart_(0)
|
||||
{
|
||||
matrix_.resize(matrix.rows(), matrix.cols());
|
||||
matrix_.triangularView<Eigen::Upper>() = matrix.triangularView<Eigen::Upper>();
|
||||
fillOffsets(dimensions.begin(), dimensions.end());
|
||||
if(matrix_.rows() != matrix_.cols())
|
||||
throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix.");
|
||||
if(variableColOffsets_.back() != matrix_.cols())
|
||||
throw std::invalid_argument("Requested to create a SymmetricBlockMatrix with dimensions that do not sum to the total size of the provided matrix.");
|
||||
assertInvariants();
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
class CholeskyFailed: public gtsam::ThreadsafeException<CholeskyFailed> {
|
||||
public:
|
||||
CholeskyFailed() throw () {
|
||||
}
|
||||
virtual ~CholeskyFailed() throw () {
|
||||
}
|
||||
};
|
||||
|
||||
/// Copy the block structure, but do not copy the matrix data. If blockStart() has been
|
||||
/// modified, this copies the structure of the corresponding matrix view. In the destination
|
||||
/// SymmetricBlockMatrix, blockStart() will be 0.
|
||||
static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& other);
|
||||
|
||||
/// Copy the block structure, but do not copy the matrix data. If blockStart() has been
|
||||
/// modified, this copies the structure of the corresponding matrix view. In the destination
|
||||
/// SymmetricBlockMatrix, blockStart() will be 0.
|
||||
static SymmetricBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& other);
|
||||
|
||||
/// Row size
|
||||
DenseIndex rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
|
||||
|
||||
/// Column size
|
||||
DenseIndex cols() const { return rows(); }
|
||||
|
||||
/// Block count
|
||||
DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
|
||||
|
||||
/// Access the block with vertical block index \c i_block and horizontal block index \c j_block.
|
||||
/// Note that the actual block accessed in the underlying matrix is relative to blockStart().
|
||||
Block operator()(DenseIndex i_block, DenseIndex j_block) {
|
||||
return Block(*this, i_block, j_block);
|
||||
}
|
||||
|
||||
/// Access the block with vertical block index \c i_block and horizontal block index \c j_block.
|
||||
/// Note that the actual block accessed in the underlying matrix is relative to blockStart().
|
||||
constBlock operator()(DenseIndex i_block, DenseIndex j_block) const {
|
||||
return constBlock(*this, i_block, j_block);
|
||||
}
|
||||
|
||||
/// Access the range of blocks starting with vertical block index \c i_startBlock, ending with
|
||||
/// vertical block index \c i_endBlock, starting with horizontal block index \c j_startBlock,
|
||||
/// and ending with horizontal block index \c j_endBlock. End block indices are exclusive. Note
|
||||
/// that the actual blocks accessed in the underlying matrix are relative to blockStart().
|
||||
Block range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) {
|
||||
assertInvariants();
|
||||
return Block(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock, j_endBlock - j_startBlock);
|
||||
}
|
||||
|
||||
/// Access the range of blocks starting with vertical block index \c i_startBlock, ending with
|
||||
/// vertical block index \c i_endBlock, starting with horizontal block index \c j_startBlock,
|
||||
/// and ending with horizontal block index \c j_endBlock. End block indices are exclusive. Note
|
||||
/// that the actual blocks accessed in the underlying matrix are relative to blockStart().
|
||||
constBlock range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const {
|
||||
assertInvariants();
|
||||
return constBlock(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock, j_endBlock - j_startBlock);
|
||||
}
|
||||
|
||||
/** Return the full matrix, *not* including any portions excluded by firstBlock(). */
|
||||
Block full()
|
||||
{
|
||||
return Block(*this, 0, nBlocks(), 0);
|
||||
}
|
||||
|
||||
/** Return the full matrix, *not* including any portions excluded by firstBlock(). */
|
||||
constBlock full() const
|
||||
{
|
||||
return constBlock(*this, 0, nBlocks(), 0);
|
||||
}
|
||||
|
||||
/** Access to full matrix, including any portions excluded by firstBlock() to other operations. */
|
||||
Eigen::SelfAdjointView<const Matrix, Eigen::Upper> matrix() const
|
||||
{
|
||||
return matrix_;
|
||||
}
|
||||
|
||||
/** Access to full matrix, including any portions excluded by firstBlock() to other operations. */
|
||||
Eigen::SelfAdjointView<Matrix, Eigen::Upper> matrix()
|
||||
{
|
||||
return matrix_;
|
||||
}
|
||||
|
||||
/// Return the absolute offset in the underlying matrix of the start of the specified \c block.
|
||||
DenseIndex offset(DenseIndex block) const
|
||||
{
|
||||
assertInvariants();
|
||||
DenseIndex actualBlock = block + blockStart_;
|
||||
checkBlock(actualBlock);
|
||||
return variableColOffsets_[actualBlock];
|
||||
}
|
||||
|
||||
/// Retrieve or modify the first logical block, i.e. the block referenced by block index 0.
|
||||
/// Blocks before it will be inaccessible, except by accessing the underlying matrix using
|
||||
/// matrix().
|
||||
DenseIndex& blockStart() { return blockStart_; }
|
||||
|
||||
/// Retrieve the first logical block, i.e. the block referenced by block index 0. Blocks before
|
||||
/// it will be inaccessible, except by accessing the underlying matrix using matrix().
|
||||
DenseIndex blockStart() const { return blockStart_; }
|
||||
|
||||
/// Do partial Cholesky in-place and return the eliminated block matrix, leaving the remaining
|
||||
/// symmetric matrix in place.
|
||||
VerticalBlockMatrix choleskyPartial(DenseIndex nFrontals);
|
||||
|
||||
protected:
|
||||
void assertInvariants() const
|
||||
{
|
||||
assert(matrix_.rows() == matrix_.cols());
|
||||
assert(matrix_.cols() == variableColOffsets_.back());
|
||||
assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
|
||||
}
|
||||
|
||||
void checkBlock(DenseIndex block) const
|
||||
{
|
||||
assert(matrix_.rows() == matrix_.cols());
|
||||
assert(matrix_.cols() == variableColOffsets_.back());
|
||||
assert(block >= 0);
|
||||
assert(block < (DenseIndex)variableColOffsets_.size()-1);
|
||||
assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
|
||||
}
|
||||
|
||||
DenseIndex offsetUnchecked(DenseIndex block) const
|
||||
{
|
||||
return variableColOffsets_[block + blockStart_];
|
||||
}
|
||||
|
||||
//void checkRange(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
|
||||
//{
|
||||
// const DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
|
||||
// const DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
|
||||
// const DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
|
||||
// const DenseIndex j_actualEndBlock = j_endBlock + blockStart_;
|
||||
// checkBlock(i_actualStartBlock);
|
||||
// checkBlock(j_actualStartBlock);
|
||||
// if(i_startBlock != 0 || i_endBlock != 0) {
|
||||
// checkBlock(i_actualStartBlock);
|
||||
// assert(i_actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
// }
|
||||
// if(j_startBlock != 0 || j_endBlock != 0) {
|
||||
// checkBlock(j_actualStartBlock);
|
||||
// assert(j_actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
// }
|
||||
//}
|
||||
|
||||
//void checkRange(DenseIndex startBlock, DenseIndex endBlock) const
|
||||
//{
|
||||
// const DenseIndex actualStartBlock = startBlock + blockStart_;
|
||||
// const DenseIndex actualEndBlock = endBlock + blockStart_;
|
||||
// checkBlock(actualStartBlock);
|
||||
// if(startBlock != 0 || endBlock != 0) {
|
||||
// checkBlock(actualStartBlock);
|
||||
// assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
// }
|
||||
//}
|
||||
|
||||
//Block rangeUnchecked(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock)
|
||||
//{
|
||||
// const DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
|
||||
// const DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
|
||||
// const DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
|
||||
// const DenseIndex j_actualEndBlock = j_endBlock + blockStart_;
|
||||
|
||||
// return Block(matrix(),
|
||||
// variableColOffsets_[i_actualStartBlock],
|
||||
// variableColOffsets_[j_actualStartBlock],
|
||||
// variableColOffsets_[i_actualEndBlock] - variableColOffsets_[i_actualStartBlock],
|
||||
// variableColOffsets_[j_actualEndBlock] - variableColOffsets_[j_actualStartBlock]);
|
||||
//}
|
||||
|
||||
//constBlock rangeUnchecked(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
|
||||
//{
|
||||
// // Convert Block to constBlock
|
||||
// const Block block = const_cast<This*>(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*>(this)->rangeUnchecked(startBlock, endBlock);
|
||||
// return constBlock(matrix(), block.startRow(), block.startCol(), block.rows(), block.cols());
|
||||
//}
|
||||
|
||||
template<typename ITERATOR>
|
||||
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim)
|
||||
{
|
||||
variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
|
||||
variableColOffsets_[0] = 0;
|
||||
DenseIndex j=0;
|
||||
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
|
||||
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
|
||||
++ j;
|
||||
}
|
||||
}
|
||||
|
||||
friend class VerticalBlockMatrix;
|
||||
template<typename SymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr;
|
||||
|
||||
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_);
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
|
||||
{
|
||||
public:
|
||||
CholeskyFailed() throw() {}
|
||||
virtual ~CholeskyFailed() throw() {}
|
||||
};
|
||||
|
||||
}
|
||||
} //\ namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -21,32 +21,32 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& other)
|
||||
{
|
||||
VerticalBlockMatrix result;
|
||||
result.variableColOffsets_.resize(other.nBlocks() + 1);
|
||||
for(size_t i = 0; i < result.variableColOffsets_.size(); ++i)
|
||||
result.variableColOffsets_[i] =
|
||||
other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_];
|
||||
result.matrix_.resize(other.rows(), result.variableColOffsets_.back());
|
||||
result.rowEnd_ = other.rows();
|
||||
result.assertInvariants();
|
||||
return result;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(
|
||||
const VerticalBlockMatrix& other) {
|
||||
VerticalBlockMatrix result;
|
||||
result.variableColOffsets_.resize(other.nBlocks() + 1);
|
||||
for (size_t i = 0; i < result.variableColOffsets_.size(); ++i)
|
||||
result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_
|
||||
+ i] - other.variableColOffsets_[other.blockStart_];
|
||||
result.matrix_.resize(other.rows(), result.variableColOffsets_.back());
|
||||
result.rowEnd_ = other.rows();
|
||||
result.assertInvariants();
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const SymmetricBlockMatrix& other, DenseIndex height)
|
||||
{
|
||||
VerticalBlockMatrix result;
|
||||
result.variableColOffsets_.resize(other.nBlocks() + 1);
|
||||
for(size_t i = 0; i < result.variableColOffsets_.size(); ++i)
|
||||
result.variableColOffsets_[i] =
|
||||
other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_];
|
||||
result.matrix_.resize(height, result.variableColOffsets_.back());
|
||||
result.rowEnd_ = height;
|
||||
result.assertInvariants();
|
||||
return result;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(
|
||||
const SymmetricBlockMatrix& other, DenseIndex height) {
|
||||
VerticalBlockMatrix result;
|
||||
result.variableColOffsets_.resize(other.nBlocks() + 1);
|
||||
for (size_t i = 0; i < result.variableColOffsets_.size(); ++i)
|
||||
result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_
|
||||
+ i] - other.variableColOffsets_[other.blockStart_];
|
||||
result.matrix_.resize(height, result.variableColOffsets_.back());
|
||||
result.rowEnd_ = height;
|
||||
result.assertInvariants();
|
||||
return result;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -22,209 +22,261 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
class SymmetricBlockMatrix;
|
||||
// Forward declarations
|
||||
class SymmetricBlockMatrix;
|
||||
|
||||
/**
|
||||
* This class stores a dense matrix and allows it to be accessed as a collection
|
||||
* of vertical blocks.
|
||||
*
|
||||
* The dimensions of the blocks are provided when constructing this class.
|
||||
*
|
||||
* This class also has three parameters that can be changed after construction
|
||||
* that change the apparent view of the matrix without any reallocation or data
|
||||
* copying. firstBlock() determines the block that has index 0 for all operations
|
||||
* (except for re-setting firstBlock()). rowStart() determines the apparent
|
||||
* first row of the matrix for all operations (except for setting rowStart() and
|
||||
* rowEnd()). rowEnd() determines the apparent exclusive (one-past-the-last)
|
||||
* last row for all operations. To include all rows, rowEnd() should be set to
|
||||
* the number of rows in the matrix (i.e. one after the last true row index).
|
||||
*
|
||||
* @addtogroup base
|
||||
*/
|
||||
class GTSAM_EXPORT VerticalBlockMatrix {
|
||||
public:
|
||||
typedef VerticalBlockMatrix This;
|
||||
typedef Eigen::Block<Matrix> Block;
|
||||
typedef Eigen::Block<const Matrix> constBlock;
|
||||
|
||||
protected:
|
||||
Matrix matrix_; ///< The full matrix
|
||||
|
||||
/// the starting columns of each block (0-based)
|
||||
FastVector<DenseIndex> variableColOffsets_;
|
||||
|
||||
DenseIndex rowStart_; ///< Changes apparent matrix view, see class comments.
|
||||
DenseIndex rowEnd_; ///< Changes apparent matrix view, see class comments.
|
||||
DenseIndex blockStart_; ///< Changes apparent matrix view, see class comments.
|
||||
|
||||
public:
|
||||
|
||||
/** Construct an empty VerticalBlockMatrix */
|
||||
VerticalBlockMatrix() :
|
||||
rowStart_(0), rowEnd_(0), blockStart_(0) {
|
||||
variableColOffsets_.push_back(0);
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct from a container of the sizes of each vertical block. */
|
||||
template<typename CONTAINER>
|
||||
VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height) :
|
||||
rowStart_(0), rowEnd_(height), blockStart_(0) {
|
||||
fillOffsets(dimensions.begin(), dimensions.end());
|
||||
matrix_.resize(height, variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/**
|
||||
* This class stores a dense matrix and allows it to be accessed as a collection of vertical
|
||||
* blocks. The dimensions of the blocks are provided when constructing this class.
|
||||
*
|
||||
* This class also has three parameters that can be changed after construction that change the
|
||||
* apparent view of the matrix without any reallocation or data copying. firstBlock() determines
|
||||
* the block that has index 0 for all operations (except for re-setting firstBlock()). rowStart()
|
||||
* determines the apparent first row of the matrix for all operations (except for setting
|
||||
* rowStart() and rowEnd()). rowEnd() determines the apparent exclusive (one-past-the-last) last
|
||||
* row for all operations. To include all rows, rowEnd() should be set to the number of rows in
|
||||
* the matrix (i.e. one after the last true row index).
|
||||
*
|
||||
* @addtogroup base */
|
||||
class GTSAM_EXPORT VerticalBlockMatrix
|
||||
{
|
||||
public:
|
||||
typedef VerticalBlockMatrix This;
|
||||
typedef Eigen::Block<Matrix> Block;
|
||||
typedef Eigen::Block<const Matrix> constBlock;
|
||||
* Construct from a container of the sizes of each vertical block and a
|
||||
* pre-prepared matrix.
|
||||
*/
|
||||
template<typename CONTAINER>
|
||||
VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) :
|
||||
matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) {
|
||||
fillOffsets(dimensions.begin(), dimensions.end());
|
||||
if (variableColOffsets_.back() != matrix_.cols())
|
||||
throw std::invalid_argument(
|
||||
"Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
protected:
|
||||
Matrix matrix_; ///< The full matrix
|
||||
FastVector<DenseIndex> variableColOffsets_; ///< the starting columns of each block (0-based)
|
||||
/**
|
||||
* Construct from iterator over the sizes of each vertical block. */
|
||||
template<typename ITERATOR>
|
||||
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim,
|
||||
DenseIndex height) :
|
||||
rowStart_(0), rowEnd_(height), blockStart_(0) {
|
||||
fillOffsets(firstBlockDim, lastBlockDim);
|
||||
matrix_.resize(height, variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
DenseIndex rowStart_; ///< Changes apparent matrix view, see main class comment.
|
||||
DenseIndex rowEnd_; ///< Changes apparent matrix view, see main class comment.
|
||||
DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment.
|
||||
/**
|
||||
* Copy the block structure and resize the underlying matrix, but do not copy
|
||||
* the matrix data. If blockStart(), rowStart(), and/or rowEnd() have been
|
||||
* modified, this copies the structure of the corresponding matrix view. In the
|
||||
* destination VerticalBlockView, blockStart() and rowStart() will thus be 0,
|
||||
* rowEnd() will be cols() of the source VerticalBlockView, and the
|
||||
* underlying matrix will be the size of the view of the source matrix.
|
||||
*/
|
||||
static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs);
|
||||
|
||||
public:
|
||||
/** Copy the block structure, but do not copy the matrix data. If blockStart()
|
||||
* has been modified, this copies the structure of the corresponding matrix
|
||||
* view. In the destination VerticalBlockMatrix, blockStart() will be 0. */
|
||||
static VerticalBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& rhs,
|
||||
DenseIndex height);
|
||||
|
||||
/** Construct an empty VerticalBlockMatrix */
|
||||
VerticalBlockMatrix() :
|
||||
rowStart_(0), rowEnd_(0), blockStart_(0)
|
||||
{
|
||||
variableColOffsets_.push_back(0);
|
||||
assertInvariants();
|
||||
/// Row size
|
||||
DenseIndex rows() const {
|
||||
assertInvariants();
|
||||
return rowEnd_ - rowStart_;
|
||||
}
|
||||
|
||||
/// Column size
|
||||
DenseIndex cols() const {
|
||||
assertInvariants();
|
||||
return variableColOffsets_.back() - variableColOffsets_[blockStart_];
|
||||
}
|
||||
|
||||
/// Block count
|
||||
DenseIndex nBlocks() const {
|
||||
assertInvariants();
|
||||
return variableColOffsets_.size() - 1 - blockStart_;
|
||||
}
|
||||
|
||||
/** Access a single block in the underlying matrix with read/write access */
|
||||
Block operator()(DenseIndex block) {
|
||||
return range(block, block + 1);
|
||||
}
|
||||
|
||||
/** Access a const block view */
|
||||
const constBlock operator()(DenseIndex block) const {
|
||||
return range(block, block + 1);
|
||||
}
|
||||
|
||||
/** access ranges of blocks at a time */
|
||||
Block range(DenseIndex startBlock, DenseIndex endBlock) {
|
||||
assertInvariants();
|
||||
DenseIndex actualStartBlock = startBlock + blockStart_;
|
||||
DenseIndex actualEndBlock = endBlock + blockStart_;
|
||||
if (startBlock != 0 || endBlock != 0) {
|
||||
checkBlock(actualStartBlock);
|
||||
assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
}
|
||||
const DenseIndex startCol = variableColOffsets_[actualStartBlock];
|
||||
const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
|
||||
return matrix_.block(rowStart_, startCol, this->rows(), rangeCols);
|
||||
}
|
||||
|
||||
/** Construct from a container of the sizes of each vertical block. */
|
||||
template<typename CONTAINER>
|
||||
VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height) :
|
||||
rowStart_(0), rowEnd_(height), blockStart_(0)
|
||||
{
|
||||
fillOffsets(dimensions.begin(), dimensions.end());
|
||||
matrix_.resize(height, variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
const constBlock range(DenseIndex startBlock, DenseIndex endBlock) const {
|
||||
assertInvariants();
|
||||
DenseIndex actualStartBlock = startBlock + blockStart_;
|
||||
DenseIndex actualEndBlock = endBlock + blockStart_;
|
||||
if (startBlock != 0 || endBlock != 0) {
|
||||
checkBlock(actualStartBlock);
|
||||
assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
}
|
||||
const DenseIndex startCol = variableColOffsets_[actualStartBlock];
|
||||
const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
|
||||
return ((const Matrix&) matrix_).block(rowStart_, startCol, this->rows(),
|
||||
rangeCols);
|
||||
}
|
||||
|
||||
/** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */
|
||||
template<typename CONTAINER>
|
||||
VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) :
|
||||
matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0)
|
||||
{
|
||||
fillOffsets(dimensions.begin(), dimensions.end());
|
||||
if(variableColOffsets_.back() != matrix_.cols())
|
||||
throw std::invalid_argument("Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
|
||||
assertInvariants();
|
||||
/** Return the full matrix, *not* including any portions excluded by
|
||||
* rowStart(), rowEnd(), and firstBlock() */
|
||||
Block full() {
|
||||
return range(0, nBlocks());
|
||||
}
|
||||
|
||||
/** Return the full matrix, *not* including any portions excluded by
|
||||
* rowStart(), rowEnd(), and firstBlock() */
|
||||
const constBlock full() const {
|
||||
return range(0, nBlocks());
|
||||
}
|
||||
|
||||
DenseIndex offset(DenseIndex block) const {
|
||||
assertInvariants();
|
||||
DenseIndex actualBlock = block + blockStart_;
|
||||
checkBlock(actualBlock);
|
||||
return variableColOffsets_[actualBlock];
|
||||
}
|
||||
|
||||
/// Get/set the apparent first row of the underlying matrix for all operations
|
||||
DenseIndex& rowStart() {
|
||||
return rowStart_;
|
||||
}
|
||||
|
||||
/** Get/set the apparent last row
|
||||
* (exclusive, i.e. rows() == rowEnd() - rowStart())
|
||||
* of the underlying matrix for all operations */
|
||||
DenseIndex& rowEnd() {
|
||||
return rowEnd_;
|
||||
}
|
||||
|
||||
/** Get/set the apparent first block for all operations */
|
||||
DenseIndex& firstBlock() {
|
||||
return blockStart_;
|
||||
}
|
||||
|
||||
/** Get the apparent first row of the underlying matrix for all operations */
|
||||
DenseIndex rowStart() const {
|
||||
return rowStart_;
|
||||
}
|
||||
|
||||
/** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart())
|
||||
* of the underlying matrix for all operations */
|
||||
DenseIndex rowEnd() const {
|
||||
return rowEnd_;
|
||||
}
|
||||
|
||||
/** Get the apparent first block for all operations */
|
||||
DenseIndex firstBlock() const {
|
||||
return blockStart_;
|
||||
}
|
||||
|
||||
/** Access to full matrix (*including* any portions excluded by rowStart(),
|
||||
* rowEnd(), and firstBlock()) */
|
||||
const Matrix& matrix() const {
|
||||
return matrix_;
|
||||
}
|
||||
|
||||
/** Non-const access to full matrix (*including* any portions excluded by
|
||||
* rowStart(), rowEnd(), and firstBlock()) */
|
||||
Matrix& matrix() {
|
||||
return matrix_;
|
||||
}
|
||||
|
||||
protected:
|
||||
void assertInvariants() const {
|
||||
assert(matrix_.cols() == variableColOffsets_.back());
|
||||
assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
|
||||
assert(rowStart_ <= matrix_.rows());
|
||||
assert(rowEnd_ <= matrix_.rows());
|
||||
assert(rowStart_ <= rowEnd_);
|
||||
}
|
||||
|
||||
void checkBlock(DenseIndex block) const {
|
||||
assert(matrix_.cols() == variableColOffsets_.back());
|
||||
assert(block < (DenseIndex)variableColOffsets_.size() - 1);
|
||||
assert(
|
||||
variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
|
||||
}
|
||||
|
||||
template<typename ITERATOR>
|
||||
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) {
|
||||
variableColOffsets_.resize((lastBlockDim - firstBlockDim) + 1);
|
||||
variableColOffsets_[0] = 0;
|
||||
DenseIndex j = 0;
|
||||
for (ITERATOR dim = firstBlockDim; dim != lastBlockDim; ++dim) {
|
||||
variableColOffsets_[j + 1] = variableColOffsets_[j] + *dim;
|
||||
++j;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct from iterator over the sizes of each vertical block. */
|
||||
template<typename ITERATOR>
|
||||
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height) :
|
||||
rowStart_(0), rowEnd_(height), blockStart_(0)
|
||||
{
|
||||
fillOffsets(firstBlockDim, lastBlockDim);
|
||||
matrix_.resize(height, variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
}
|
||||
friend class SymmetricBlockMatrix;
|
||||
|
||||
/** Copy the block structure and resize the underlying matrix, but do not copy the matrix data.
|
||||
* If blockStart(), rowStart(), and/or rowEnd() have been modified, this copies the structure of
|
||||
* the corresponding matrix view. In the destination VerticalBlockView, blockStart() and
|
||||
* rowStart() will thus be 0, rowEnd() will be cols() of the source VerticalBlockView, and the
|
||||
* underlying matrix will be the size of the view of the source matrix. */
|
||||
static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs);
|
||||
|
||||
/** Copy the block structure, but do not copy the matrix data. If blockStart() has been
|
||||
* modified, this copies the structure of the corresponding matrix view. In the destination
|
||||
* VerticalBlockMatrix, blockStart() will be 0. */
|
||||
static VerticalBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& rhs, DenseIndex height);
|
||||
|
||||
/// Row size
|
||||
DenseIndex rows() const { assertInvariants(); return rowEnd_ - rowStart_; }
|
||||
|
||||
/// Column size
|
||||
DenseIndex cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
|
||||
|
||||
/// Block count
|
||||
DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
|
||||
|
||||
/** Access a single block in the underlying matrix with read/write access */
|
||||
Block operator()(DenseIndex block) { return range(block, block+1); }
|
||||
|
||||
/** Access a const block view */
|
||||
const constBlock operator()(DenseIndex block) const { return range(block, block+1); }
|
||||
|
||||
/** access ranges of blocks at a time */
|
||||
Block range(DenseIndex startBlock, DenseIndex endBlock) {
|
||||
assertInvariants();
|
||||
DenseIndex actualStartBlock = startBlock + blockStart_;
|
||||
DenseIndex actualEndBlock = endBlock + blockStart_;
|
||||
if(startBlock != 0 || endBlock != 0) {
|
||||
checkBlock(actualStartBlock);
|
||||
assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
}
|
||||
const DenseIndex startCol = variableColOffsets_[actualStartBlock];
|
||||
const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
|
||||
return matrix_.block(rowStart_, startCol, this->rows(), rangeCols);
|
||||
}
|
||||
|
||||
const constBlock range(DenseIndex startBlock, DenseIndex endBlock) const {
|
||||
assertInvariants();
|
||||
DenseIndex actualStartBlock = startBlock + blockStart_;
|
||||
DenseIndex actualEndBlock = endBlock + blockStart_;
|
||||
if(startBlock != 0 || endBlock != 0) {
|
||||
checkBlock(actualStartBlock);
|
||||
assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
}
|
||||
const DenseIndex startCol = variableColOffsets_[actualStartBlock];
|
||||
const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
|
||||
return ((const Matrix&)matrix_).block(rowStart_, startCol, this->rows(), rangeCols);
|
||||
}
|
||||
|
||||
/** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */
|
||||
Block full() { return range(0, nBlocks()); }
|
||||
|
||||
/** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */
|
||||
const constBlock full() const { return range(0, nBlocks()); }
|
||||
|
||||
DenseIndex offset(DenseIndex block) const {
|
||||
assertInvariants();
|
||||
DenseIndex actualBlock = block + blockStart_;
|
||||
checkBlock(actualBlock);
|
||||
return variableColOffsets_[actualBlock];
|
||||
}
|
||||
|
||||
/** Get or set the apparent first row of the underlying matrix for all operations */
|
||||
DenseIndex& rowStart() { return rowStart_; }
|
||||
|
||||
/** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
|
||||
DenseIndex& rowEnd() { return rowEnd_; }
|
||||
|
||||
/** Get or set the apparent first block for all operations */
|
||||
DenseIndex& firstBlock() { return blockStart_; }
|
||||
|
||||
/** Get the apparent first row of the underlying matrix for all operations */
|
||||
DenseIndex rowStart() const { return rowStart_; }
|
||||
|
||||
/** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
|
||||
DenseIndex rowEnd() const { return rowEnd_; }
|
||||
|
||||
/** Get the apparent first block for all operations */
|
||||
DenseIndex firstBlock() const { return blockStart_; }
|
||||
|
||||
/** Access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */
|
||||
const Matrix& matrix() const { return matrix_; }
|
||||
|
||||
/** Non-const access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */
|
||||
Matrix& matrix() { return matrix_; }
|
||||
|
||||
protected:
|
||||
void assertInvariants() const {
|
||||
assert(matrix_.cols() == variableColOffsets_.back());
|
||||
assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
|
||||
assert(rowStart_ <= matrix_.rows());
|
||||
assert(rowEnd_ <= matrix_.rows());
|
||||
assert(rowStart_ <= rowEnd_);
|
||||
}
|
||||
|
||||
void checkBlock(DenseIndex block) const {
|
||||
assert(matrix_.cols() == variableColOffsets_.back());
|
||||
assert(block < (DenseIndex)variableColOffsets_.size() - 1);
|
||||
assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
|
||||
}
|
||||
|
||||
template<typename ITERATOR>
|
||||
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) {
|
||||
variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
|
||||
variableColOffsets_[0] = 0;
|
||||
DenseIndex j=0;
|
||||
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
|
||||
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
|
||||
++ j;
|
||||
}
|
||||
}
|
||||
|
||||
friend class SymmetricBlockMatrix;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<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_);
|
||||
}
|
||||
};
|
||||
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_);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,47 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testVerticalBlockMatrix.cpp
|
||||
* @brief Unit tests for VerticalBlockMatrix class
|
||||
* @author Frank Dellaert
|
||||
* @date February 15, 2014
|
||||
**/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/VerticalBlockMatrix.h>
|
||||
#include <boost/assign/list_of.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using boost::assign::list_of;
|
||||
|
||||
//*****************************************************************************
|
||||
TEST(VerticalBlockMatrix, constructor) {
|
||||
VerticalBlockMatrix actual(list_of(3)(2)(1),
|
||||
(Matrix(6, 6) << 1, 2, 3, 4, 5, 6, //
|
||||
2, 8, 9, 10, 11, 12, //
|
||||
3, 9, 15, 16, 17, 18, //
|
||||
4, 10, 16, 22, 23, 24, //
|
||||
5, 11, 17, 23, 29, 30, //
|
||||
6, 12, 18, 24, 30, 36));
|
||||
EXPECT_LONGS_EQUAL(6,actual.rows());
|
||||
EXPECT_LONGS_EQUAL(6,actual.cols());
|
||||
EXPECT_LONGS_EQUAL(3,actual.nBlocks());
|
||||
}
|
||||
|
||||
//*****************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
//*****************************************************************************
|
||||
|
||||
Loading…
Reference in New Issue