Merge branch 'origin/feature/LM/SymmetricBlockMatrix'

release/4.3a0
dellaert 2014-02-15 12:20:06 -05:00
commit ba6f8b7cca
6 changed files with 748 additions and 573 deletions

View File

@ -2092,6 +2092,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>

View File

@ -1,20 +1,20 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * 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 * @file SymmetricBlockMatrix.cpp
* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional. * @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional.
* @author Richard Roberts * @author Richard Roberts
* @date Sep 18, 2010 * @date Sep 18, 2010
*/ */
#include <gtsam/base/SymmetricBlockMatrix.h> #include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/VerticalBlockMatrix.h> #include <gtsam/base/VerticalBlockMatrix.h>
@ -23,39 +23,40 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(const SymmetricBlockMatrix& other) SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(
{ const SymmetricBlockMatrix& other) {
SymmetricBlockMatrix result; SymmetricBlockMatrix result;
result.variableColOffsets_.resize(other.nBlocks() + 1); result.variableColOffsets_.resize(other.nBlocks() + 1);
for(size_t i = 0; i < result.variableColOffsets_.size(); ++i) for (size_t i = 0; i < result.variableColOffsets_.size(); ++i)
result.variableColOffsets_[i] = result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_
other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_]; + i] - other.variableColOffsets_[other.blockStart_];
result.matrix_.resize(other.cols(), other.cols()); result.matrix_.resize(other.cols(), other.cols());
result.assertInvariants(); result.assertInvariants();
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& other) SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(
{ const VerticalBlockMatrix& other) {
SymmetricBlockMatrix result; SymmetricBlockMatrix result;
result.variableColOffsets_.resize(other.nBlocks() + 1); result.variableColOffsets_.resize(other.nBlocks() + 1);
for(size_t i = 0; i < result.variableColOffsets_.size(); ++i) for (size_t i = 0; i < result.variableColOffsets_.size(); ++i)
result.variableColOffsets_[i] = result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_
other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_]; + i] - other.variableColOffsets_[other.blockStart_];
result.matrix_.resize(other.cols(), other.cols()); result.matrix_.resize(other.cols(), other.cols());
result.assertInvariants(); result.assertInvariants();
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial(DenseIndex nFrontals) VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial(
{ DenseIndex nFrontals) {
// Do dense elimination // Do dense elimination
if(!blockStart() == 0) if (!blockStart() == 0)
throw std::invalid_argument("Can only do Cholesky when the SymmetricBlockMatrix is not a restricted view, i.e. when blockStart == 0."); throw std::invalid_argument(
if(!gtsam::choleskyPartial(matrix_, offset(nFrontals))) "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(); throw CholeskyFailed();
// Split conditional // Split conditional
@ -74,5 +75,8 @@ namespace gtsam {
gttoc(Remaining_factor); gttoc(Remaining_factor);
return Ab; return Ab;
}
} }
/* ************************************************************************* */
} //\ namespace gtsam

View File

@ -1,20 +1,20 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * 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 * @file SymmetricBlockMatrix.h
* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional. * @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional.
* @author Richard Roberts * @author Richard Roberts
* @date Sep 18, 2010 * @date Sep 18, 2010
*/ */
#pragma once #pragma once
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
@ -23,38 +23,42 @@
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
class VerticalBlockMatrix; class VerticalBlockMatrix;
/** /**
* This class stores a dense matrix and allows it to be accessed as a collection of blocks. When * This class stores a dense matrix and allows it to be accessed as a
* constructed, the caller must provide the dimensions of the blocks. * 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. * 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 * This class also has a parameter that can be changed after construction to
* matrix view. firstBlock() determines the block that appears to have index 0 for all operations * change the apparent matrix view. firstBlock() determines the block that
* (except re-setting firstBlock()). * appears to have index 0 for all operations (except re-setting firstBlock).
* *
* @addtogroup base */ * @addtogroup base */
class GTSAM_EXPORT SymmetricBlockMatrix class GTSAM_EXPORT SymmetricBlockMatrix {
{ public:
public:
typedef SymmetricBlockMatrix This; typedef SymmetricBlockMatrix This;
typedef SymmetricBlockMatrixBlockExpr<This> Block; typedef SymmetricBlockMatrixBlockExpr<This> Block;
typedef SymmetricBlockMatrixBlockExpr<const This> constBlock; typedef SymmetricBlockMatrixBlockExpr<const This> constBlock;
protected: protected:
Matrix matrix_; ///< The full matrix Matrix matrix_; ///< The full matrix
FastVector<DenseIndex> variableColOffsets_; ///< the starting columns of each block (0-based)
DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment. /// the starting columns of each block (0-based)
FastVector<DenseIndex> variableColOffsets_;
public: /// Changes apparent matrix view, see main class comment.
DenseIndex blockStart_;
public:
/// Construct from an empty matrix (asserts that the matrix is empty) /// Construct from an empty matrix (asserts that the matrix is empty)
SymmetricBlockMatrix() : SymmetricBlockMatrix() :
blockStart_(0) blockStart_(0) {
{
variableColOffsets_.push_back(0); variableColOffsets_.push_back(0);
assertInvariants(); assertInvariants();
} }
@ -62,8 +66,7 @@ namespace gtsam {
/// Construct from a container of the sizes of each block. /// Construct from a container of the sizes of each block.
template<typename CONTAINER> template<typename CONTAINER>
SymmetricBlockMatrix(const CONTAINER& dimensions) : SymmetricBlockMatrix(const CONTAINER& dimensions) :
blockStart_(0) blockStart_(0) {
{
fillOffsets(dimensions.begin(), dimensions.end()); fillOffsets(dimensions.begin(), dimensions.end());
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
assertInvariants(); assertInvariants();
@ -72,142 +75,195 @@ namespace gtsam {
/// Construct from iterator over the sizes of each vertical block. /// Construct from iterator over the sizes of each vertical block.
template<typename ITERATOR> template<typename ITERATOR>
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim) : SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim) :
blockStart_(0) blockStart_(0) {
{
fillOffsets(firstBlockDim, lastBlockDim); fillOffsets(firstBlockDim, lastBlockDim);
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
assertInvariants(); assertInvariants();
} }
/// Construct from a container of the sizes of each vertical block and a pre-prepared matrix. /**
* @brief Construct from a container of the sizes of each vertical block
* and a pre-prepared matrix.
*/
template<typename CONTAINER> template<typename CONTAINER>
SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) : SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) :
blockStart_(0) blockStart_(0) {
{
matrix_.resize(matrix.rows(), matrix.cols()); matrix_.resize(matrix.rows(), matrix.cols());
matrix_.triangularView<Eigen::Upper>() = matrix.triangularView<Eigen::Upper>(); matrix_.triangularView<Eigen::Upper>() =
matrix.triangularView<Eigen::Upper>();
fillOffsets(dimensions.begin(), dimensions.end()); fillOffsets(dimensions.begin(), dimensions.end());
if(matrix_.rows() != matrix_.cols()) if (matrix_.rows() != matrix_.cols())
throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix."); throw std::invalid_argument("Requested to create a SymmetricBlockMatrix"
if(variableColOffsets_.back() != matrix_.cols()) " from a non-square matrix.");
throw std::invalid_argument("Requested to create a SymmetricBlockMatrix with dimensions that do not sum to the total size of the provided 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(); assertInvariants();
} }
/// Copy the block structure, but do not copy the matrix data. If blockStart() has been /**
/// modified, this copies the structure of the corresponding matrix view. In the destination * Copy the block structure, but do not copy the matrix data. If blockStart()
/// SymmetricBlockMatrix, blockStart() will be 0. * has been modified, this copies the structure of the corresponding matrix.
static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& other); * 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 * Copy the block structure, but do not copy the matrix data. If blockStart()
/// SymmetricBlockMatrix, blockStart() will be 0. * has been modified, this copies the structure of the corresponding matrix.
static SymmetricBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& other); * In the destination SymmetricBlockMatrix, blockStart() will be 0.
*/
static SymmetricBlockMatrix LikeActiveViewOf(
const VerticalBlockMatrix& other);
/// Row size /// Row size
DenseIndex rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } DenseIndex rows() const {
assertInvariants();
return variableColOffsets_.back() - variableColOffsets_[blockStart_];
}
/// Column size /// Column size
DenseIndex cols() const { return rows(); } DenseIndex cols() const {
return rows();
}
/// Block count /// Block count
DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } 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(). * 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) { Block operator()(DenseIndex i_block, DenseIndex j_block) {
return Block(*this, i_block, 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(). * 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 { constBlock operator()(DenseIndex i_block, DenseIndex j_block) const {
return constBlock(*this, i_block, j_block); 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, * Access the range of blocks starting with vertical block index
/// and ending with horizontal block index \c j_endBlock. End block indices are exclusive. Note * \c i_startBlock, ending with vertical block index \c i_endBlock, starting
/// that the actual blocks accessed in the underlying matrix are relative to blockStart(). * with horizontal block index \c j_startBlock, and ending with horizontal
Block range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) { * 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(); assertInvariants();
return Block(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock, j_endBlock - j_startBlock); 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, * Access the range of blocks starting with vertical block index
/// and ending with horizontal block index \c j_endBlock. End block indices are exclusive. Note * \c i_startBlock, ending with vertical block index \c i_endBlock, starting
/// that the actual blocks accessed in the underlying matrix are relative to blockStart(). * with horizontal block index \c j_startBlock, and ending with horizontal
constBlock range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const { * 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(); assertInvariants();
return constBlock(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock, j_endBlock - j_startBlock); 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 the full matrix, *not* including any portions excluded by
{ * firstBlock().
*/
Block full() {
return Block(*this, 0, nBlocks(), 0); return Block(*this, 0, nBlocks(), 0);
} }
/** Return the full matrix, *not* including any portions excluded by firstBlock(). */ /**
constBlock full() const * Return the full matrix, *not* including any portions excluded by
{ * firstBlock().
*/
constBlock full() const {
return constBlock(*this, 0, nBlocks(), 0); 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 * Access to full matrix, including any portions excluded by firstBlock()
{ * to other operations.
*/
Eigen::SelfAdjointView<const Matrix, Eigen::Upper> matrix() const {
return matrix_; return matrix_;
} }
/** Access to full matrix, including any portions excluded by firstBlock() to other operations. */ /** Access to full matrix, including any portions excluded by firstBlock()
Eigen::SelfAdjointView<Matrix, Eigen::Upper> matrix() * to other operations.
{ */
Eigen::SelfAdjointView<Matrix, Eigen::Upper> matrix() {
return matrix_; return matrix_;
} }
/// Return the absolute offset in the underlying matrix of the start of the specified \c block. /**
DenseIndex offset(DenseIndex block) const * Return the absolute offset in the underlying matrix
{ * of the start of the specified \c block.
*/
DenseIndex offset(DenseIndex block) const {
assertInvariants(); assertInvariants();
DenseIndex actualBlock = block + blockStart_; DenseIndex actualBlock = block + blockStart_;
checkBlock(actualBlock); checkBlock(actualBlock);
return variableColOffsets_[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 * Retrieve or modify the first logical block, i.e. the block referenced by
/// matrix(). * block index 0. Blocks before it will be inaccessible, except by accessing
DenseIndex& blockStart() { return blockStart_; } * 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(). * Retrieve the first logical block, i.e. the block referenced by block index 0.
DenseIndex blockStart() const { return blockStart_; } * 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. * Do partial Cholesky in-place and return the eliminated block matrix,
* leaving the remaining symmetric matrix in place.
*/
VerticalBlockMatrix choleskyPartial(DenseIndex nFrontals); VerticalBlockMatrix choleskyPartial(DenseIndex nFrontals);
protected: protected:
void assertInvariants() const void assertInvariants() const {
{
assert(matrix_.rows() == matrix_.cols()); assert(matrix_.rows() == matrix_.cols());
assert(matrix_.cols() == variableColOffsets_.back()); assert(matrix_.cols() == variableColOffsets_.back());
assert(blockStart_ < (DenseIndex)variableColOffsets_.size()); assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
} }
void checkBlock(DenseIndex block) const void checkBlock(DenseIndex block) const {
{
assert(matrix_.rows() == matrix_.cols()); assert(matrix_.rows() == matrix_.cols());
assert(matrix_.cols() == variableColOffsets_.back()); assert(matrix_.cols() == variableColOffsets_.back());
assert(block >= 0); assert(block >= 0);
assert(block < (DenseIndex)variableColOffsets_.size()-1); assert(block < (DenseIndex)variableColOffsets_.size()-1);
assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols()); assert(
variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
} }
DenseIndex offsetUnchecked(DenseIndex block) const DenseIndex offsetUnchecked(DenseIndex block) const {
{
return variableColOffsets_[block + blockStart_]; return variableColOffsets_[block + blockStart_];
} }
@ -281,21 +337,20 @@ namespace gtsam {
//} //}
template<typename ITERATOR> template<typename ITERATOR>
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) {
{ variableColOffsets_.resize((lastBlockDim - firstBlockDim) + 1);
variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
variableColOffsets_[0] = 0; variableColOffsets_[0] = 0;
DenseIndex j=0; DenseIndex j = 0;
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { for (ITERATOR dim = firstBlockDim; dim != lastBlockDim; ++dim) {
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; variableColOffsets_[j + 1] = variableColOffsets_[j] + *dim;
++ j; ++j;
} }
} }
friend class VerticalBlockMatrix; friend class VerticalBlockMatrix;
template<typename SymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr; template<typename SymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
@ -304,15 +359,16 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
ar & BOOST_SERIALIZATION_NVP(blockStart_); ar & BOOST_SERIALIZATION_NVP(blockStart_);
} }
}; };
/* ************************************************************************* */ /* ************************************************************************* */
class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed> class CholeskyFailed: public gtsam::ThreadsafeException<CholeskyFailed> {
{ public:
public: CholeskyFailed() throw () {
CholeskyFailed() throw() {} }
virtual ~CholeskyFailed() throw() {} virtual ~CholeskyFailed() throw () {
}; }
};
} } //\ namespace gtsam

View File

@ -21,32 +21,32 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& other) VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(
{ const VerticalBlockMatrix& other) {
VerticalBlockMatrix result; VerticalBlockMatrix result;
result.variableColOffsets_.resize(other.nBlocks() + 1); result.variableColOffsets_.resize(other.nBlocks() + 1);
for(size_t i = 0; i < result.variableColOffsets_.size(); ++i) for (size_t i = 0; i < result.variableColOffsets_.size(); ++i)
result.variableColOffsets_[i] = result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_
other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_]; + i] - other.variableColOffsets_[other.blockStart_];
result.matrix_.resize(other.rows(), result.variableColOffsets_.back()); result.matrix_.resize(other.rows(), result.variableColOffsets_.back());
result.rowEnd_ = other.rows(); result.rowEnd_ = other.rows();
result.assertInvariants(); result.assertInvariants();
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const SymmetricBlockMatrix& other, DenseIndex height) VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(
{ const SymmetricBlockMatrix& other, DenseIndex height) {
VerticalBlockMatrix result; VerticalBlockMatrix result;
result.variableColOffsets_.resize(other.nBlocks() + 1); result.variableColOffsets_.resize(other.nBlocks() + 1);
for(size_t i = 0; i < result.variableColOffsets_.size(); ++i) for (size_t i = 0; i < result.variableColOffsets_.size(); ++i)
result.variableColOffsets_[i] = result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_
other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_]; + i] - other.variableColOffsets_[other.blockStart_];
result.matrix_.resize(height, result.variableColOffsets_.back()); result.matrix_.resize(height, result.variableColOffsets_.back());
result.rowEnd_ = height; result.rowEnd_ = height;
result.assertInvariants(); result.assertInvariants();
return result; return result;
} }
} }

View File

@ -22,43 +22,47 @@
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
class SymmetricBlockMatrix; class SymmetricBlockMatrix;
/** /**
* This class stores a dense matrix and allows it to be accessed as a collection of vertical * This class stores a dense matrix and allows it to be accessed as a collection
* blocks. The dimensions of the blocks are provided when constructing this class. * of vertical blocks.
* *
* This class also has three parameters that can be changed after construction that change the * The dimensions of the blocks are provided when constructing this class.
* 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 */ * This class also has three parameters that can be changed after construction
class GTSAM_EXPORT VerticalBlockMatrix * 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
public: * (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 VerticalBlockMatrix This;
typedef Eigen::Block<Matrix> Block; typedef Eigen::Block<Matrix> Block;
typedef Eigen::Block<const Matrix> constBlock; typedef Eigen::Block<const Matrix> constBlock;
protected: protected:
Matrix matrix_; ///< The full matrix Matrix matrix_; ///< The full matrix
FastVector<DenseIndex> variableColOffsets_; ///< the starting columns of each block (0-based)
DenseIndex rowStart_; ///< Changes apparent matrix view, see main class comment. /// the starting columns of each block (0-based)
DenseIndex rowEnd_; ///< Changes apparent matrix view, see main class comment. FastVector<DenseIndex> variableColOffsets_;
DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment.
public: 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 */ /** Construct an empty VerticalBlockMatrix */
VerticalBlockMatrix() : VerticalBlockMatrix() :
rowStart_(0), rowEnd_(0), blockStart_(0) rowStart_(0), rowEnd_(0), blockStart_(0) {
{
variableColOffsets_.push_back(0); variableColOffsets_.push_back(0);
assertInvariants(); assertInvariants();
} }
@ -66,68 +70,87 @@ namespace gtsam {
/** Construct from a container of the sizes of each vertical block. */ /** Construct from a container of the sizes of each vertical block. */
template<typename CONTAINER> template<typename CONTAINER>
VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height) : VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height) :
rowStart_(0), rowEnd_(height), blockStart_(0) rowStart_(0), rowEnd_(height), blockStart_(0) {
{
fillOffsets(dimensions.begin(), dimensions.end()); fillOffsets(dimensions.begin(), dimensions.end());
matrix_.resize(height, variableColOffsets_.back()); matrix_.resize(height, variableColOffsets_.back());
assertInvariants(); assertInvariants();
} }
/** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ /**
* Construct from a container of the sizes of each vertical block and a
* pre-prepared matrix.
*/
template<typename CONTAINER> template<typename CONTAINER>
VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) : VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) :
matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) {
{
fillOffsets(dimensions.begin(), dimensions.end()); fillOffsets(dimensions.begin(), dimensions.end());
if(variableColOffsets_.back() != matrix_.cols()) 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."); throw std::invalid_argument(
"Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
assertInvariants(); assertInvariants();
} }
/** /**
* Construct from iterator over the sizes of each vertical block. */ * Construct from iterator over the sizes of each vertical block. */
template<typename ITERATOR> template<typename ITERATOR>
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height) : VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim,
rowStart_(0), rowEnd_(height), blockStart_(0) DenseIndex height) :
{ rowStart_(0), rowEnd_(height), blockStart_(0) {
fillOffsets(firstBlockDim, lastBlockDim); fillOffsets(firstBlockDim, lastBlockDim);
matrix_.resize(height, variableColOffsets_.back()); matrix_.resize(height, variableColOffsets_.back());
assertInvariants(); assertInvariants();
} }
/** 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 * Copy the block structure and resize the underlying matrix, but do not copy
* the corresponding matrix view. In the destination VerticalBlockView, blockStart() and * the matrix data. If blockStart(), rowStart(), and/or rowEnd() have been
* rowStart() will thus be 0, rowEnd() will be cols() of the source VerticalBlockView, and the * modified, this copies the structure of the corresponding matrix view. In the
* underlying matrix will be the size of the view of the source matrix. */ * 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); static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs);
/** Copy the block structure, but do not copy the matrix data. If blockStart() has been /** Copy the block structure, but do not copy the matrix data. If blockStart()
* modified, this copies the structure of the corresponding matrix view. In the destination * has been modified, this copies the structure of the corresponding matrix
* VerticalBlockMatrix, blockStart() will be 0. */ * view. In the destination VerticalBlockMatrix, blockStart() will be 0. */
static VerticalBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& rhs, DenseIndex height); static VerticalBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& rhs,
DenseIndex height);
/// Row size /// Row size
DenseIndex rows() const { assertInvariants(); return rowEnd_ - rowStart_; } DenseIndex rows() const {
assertInvariants();
return rowEnd_ - rowStart_;
}
/// Column size /// Column size
DenseIndex cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } DenseIndex cols() const {
assertInvariants();
return variableColOffsets_.back() - variableColOffsets_[blockStart_];
}
/// Block count /// Block count
DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } DenseIndex nBlocks() const {
assertInvariants();
return variableColOffsets_.size() - 1 - blockStart_;
}
/** Access a single block in the underlying matrix with read/write access */ /** Access a single block in the underlying matrix with read/write access */
Block operator()(DenseIndex block) { return range(block, block+1); } Block operator()(DenseIndex block) {
return range(block, block + 1);
}
/** Access a const block view */ /** Access a const block view */
const constBlock operator()(DenseIndex block) const { return range(block, block+1); } const constBlock operator()(DenseIndex block) const {
return range(block, block + 1);
}
/** access ranges of blocks at a time */ /** access ranges of blocks at a time */
Block range(DenseIndex startBlock, DenseIndex endBlock) { Block range(DenseIndex startBlock, DenseIndex endBlock) {
assertInvariants(); assertInvariants();
DenseIndex actualStartBlock = startBlock + blockStart_; DenseIndex actualStartBlock = startBlock + blockStart_;
DenseIndex actualEndBlock = endBlock + blockStart_; DenseIndex actualEndBlock = endBlock + blockStart_;
if(startBlock != 0 || endBlock != 0) { if (startBlock != 0 || endBlock != 0) {
checkBlock(actualStartBlock); checkBlock(actualStartBlock);
assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
} }
@ -140,20 +163,27 @@ namespace gtsam {
assertInvariants(); assertInvariants();
DenseIndex actualStartBlock = startBlock + blockStart_; DenseIndex actualStartBlock = startBlock + blockStart_;
DenseIndex actualEndBlock = endBlock + blockStart_; DenseIndex actualEndBlock = endBlock + blockStart_;
if(startBlock != 0 || endBlock != 0) { if (startBlock != 0 || endBlock != 0) {
checkBlock(actualStartBlock); checkBlock(actualStartBlock);
assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
} }
const DenseIndex startCol = variableColOffsets_[actualStartBlock]; const DenseIndex startCol = variableColOffsets_[actualStartBlock];
const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol; const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
return ((const Matrix&)matrix_).block(rowStart_, startCol, this->rows(), rangeCols); return ((const Matrix&) matrix_).block(rowStart_, startCol, this->rows(),
rangeCols);
} }
/** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */ /** Return the full matrix, *not* including any portions excluded by
Block full() { return range(0, nBlocks()); } * rowStart(), rowEnd(), and firstBlock() */
Block full() {
return range(0, nBlocks());
}
/** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */ /** Return the full matrix, *not* including any portions excluded by
const constBlock full() const { return range(0, nBlocks()); } * rowStart(), rowEnd(), and firstBlock() */
const constBlock full() const {
return range(0, nBlocks());
}
DenseIndex offset(DenseIndex block) const { DenseIndex offset(DenseIndex block) const {
assertInvariants(); assertInvariants();
@ -162,31 +192,52 @@ namespace gtsam {
return variableColOffsets_[actualBlock]; return variableColOffsets_[actualBlock];
} }
/** Get or set the apparent first row of the underlying matrix for all operations */ /// Get/set the apparent first row of the underlying matrix for all operations
DenseIndex& rowStart() { return rowStart_; } DenseIndex& rowStart() {
return rowStart_;
}
/** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ /** Get/set the apparent last row
DenseIndex& rowEnd() { return rowEnd_; } * (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 */ /** Get/set the apparent first block for all operations */
DenseIndex& firstBlock() { return blockStart_; } DenseIndex& firstBlock() {
return blockStart_;
}
/** Get the apparent first row of the underlying matrix for all operations */ /** Get the apparent first row of the underlying matrix for all operations */
DenseIndex rowStart() const { return rowStart_; } DenseIndex rowStart() const {
return rowStart_;
}
/** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ /** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart())
DenseIndex rowEnd() const { return rowEnd_; } * of the underlying matrix for all operations */
DenseIndex rowEnd() const {
return rowEnd_;
}
/** Get the apparent first block for all operations */ /** Get the apparent first block for all operations */
DenseIndex firstBlock() const { return blockStart_; } DenseIndex firstBlock() const {
return blockStart_;
}
/** Access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */ /** Access to full matrix (*including* any portions excluded by rowStart(),
const Matrix& matrix() const { return matrix_; } * rowEnd(), and firstBlock()) */
const Matrix& matrix() const {
return matrix_;
}
/** Non-const access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */ /** Non-const access to full matrix (*including* any portions excluded by
Matrix& matrix() { return matrix_; } * rowStart(), rowEnd(), and firstBlock()) */
Matrix& matrix() {
return matrix_;
}
protected: protected:
void assertInvariants() const { void assertInvariants() const {
assert(matrix_.cols() == variableColOffsets_.back()); assert(matrix_.cols() == variableColOffsets_.back());
assert(blockStart_ < (DenseIndex)variableColOffsets_.size()); assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
@ -198,23 +249,24 @@ namespace gtsam {
void checkBlock(DenseIndex block) const { void checkBlock(DenseIndex block) const {
assert(matrix_.cols() == variableColOffsets_.back()); assert(matrix_.cols() == variableColOffsets_.back());
assert(block < (DenseIndex)variableColOffsets_.size() - 1); assert(block < (DenseIndex)variableColOffsets_.size() - 1);
assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols()); assert(
variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
} }
template<typename ITERATOR> template<typename ITERATOR>
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) { void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) {
variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1); variableColOffsets_.resize((lastBlockDim - firstBlockDim) + 1);
variableColOffsets_[0] = 0; variableColOffsets_[0] = 0;
DenseIndex j=0; DenseIndex j = 0;
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { for (ITERATOR dim = firstBlockDim; dim != lastBlockDim; ++dim) {
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; variableColOffsets_[j + 1] = variableColOffsets_[j] + *dim;
++ j; ++j;
} }
} }
friend class SymmetricBlockMatrix; friend class SymmetricBlockMatrix;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
@ -225,6 +277,6 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(rowEnd_); ar & BOOST_SERIALIZATION_NVP(rowEnd_);
ar & BOOST_SERIALIZATION_NVP(blockStart_); ar & BOOST_SERIALIZATION_NVP(blockStart_);
} }
}; };
} }

View File

@ -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);
}
//*****************************************************************************