Format using BORG conventions

release/4.3a0
dellaert 2014-02-15 11:55:00 -05:00
parent b7cbc7a66f
commit 99947c7c6d
1 changed files with 224 additions and 191 deletions

View File

@ -38,8 +38,7 @@ namespace gtsam {
* the matrix (i.e. one after the last true row index). * the matrix (i.e. one after the last true row index).
* *
* @addtogroup base */ * @addtogroup base */
class GTSAM_EXPORT VerticalBlockMatrix class GTSAM_EXPORT VerticalBlockMatrix {
{
public: public:
typedef VerticalBlockMatrix This; typedef VerticalBlockMatrix This;
typedef Eigen::Block<Matrix> Block; typedef Eigen::Block<Matrix> Block;
@ -57,8 +56,7 @@ namespace gtsam {
/** 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,8 +64,7 @@ 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();
@ -76,20 +73,20 @@ namespace gtsam {
/** 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();
@ -105,22 +102,36 @@ namespace gtsam {
/** 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() has been
* modified, this copies the structure of the corresponding matrix view. In the destination * modified, this copies the structure of the corresponding matrix view. In the destination
* VerticalBlockMatrix, blockStart() will be 0. */ * 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) {
@ -146,14 +157,19 @@ namespace gtsam {
} }
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 rowStart(), rowEnd(), and firstBlock() */
Block full() { return range(0, nBlocks()); } 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 rowStart(), rowEnd(), and firstBlock() */
const constBlock full() const { return range(0, nBlocks()); } const constBlock full() const {
return range(0, nBlocks());
}
DenseIndex offset(DenseIndex block) const { DenseIndex offset(DenseIndex block) const {
assertInvariants(); assertInvariants();
@ -163,28 +179,44 @@ namespace gtsam {
} }
/** Get or set the apparent first row of the underlying matrix for all operations */ /** Get or 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 or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
DenseIndex& rowEnd() { return rowEnd_; } DenseIndex& rowEnd() {
return rowEnd_;
}
/** Get or set the apparent first block for all operations */ /** Get or 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()) of the underlying matrix for all operations */
DenseIndex rowEnd() const { return rowEnd_; } 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(), rowEnd(), and firstBlock()) */
const Matrix& matrix() const { return matrix_; } 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 rowStart(), rowEnd(), and firstBlock()) */
Matrix& matrix() { return matrix_; } Matrix& matrix() {
return matrix_;
}
protected: protected:
void assertInvariants() const { void assertInvariants() const {
@ -198,7 +230,8 @@ 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>