diff --git a/gtsam/base/VerticalBlockMatrix.cpp b/gtsam/base/VerticalBlockMatrix.cpp index abea56bac..1c75387a3 100644 --- a/gtsam/base/VerticalBlockMatrix.cpp +++ b/gtsam/base/VerticalBlockMatrix.cpp @@ -24,7 +24,7 @@ namespace gtsam { VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& rhs) { VerticalBlockMatrix result; result.variableColOffsets_.resize(rhs.nBlocks() + 1); - for(Index i = 0; i < rhs.nBlocks(); ++i) + for(Index i = 0; i < rhs.nBlocks() + 1; ++i) result.variableColOffsets_[i] = rhs.variableColOffsets_[i + rhs.blockStart_]; result.matrix_.resize(rhs.rows(), result.variableColOffsets_.back()); result.rowEnd_ = rhs.rows(); diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 9c3e3b2ea..f2598e67d 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -71,10 +71,10 @@ namespace gtsam { * height is matrixNewHeight and its width fits the given block dimensions. */ template VerticalBlockMatrix(const CONTAINER dimensions, DenseIndex height) : - matrix_(height, variableColOffsets_.back()), rowStart_(0), rowEnd_(height), blockStart_(0) { fillOffsets(dimensions.begin(), dimensions.end()); + matrix_.resize(height, variableColOffsets_.back()); assertInvariants(); } @@ -83,10 +83,10 @@ namespace gtsam { * height is matrixNewHeight and its width fits the given block dimensions. */ template VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height) : - matrix_(height, variableColOffsets_.back()), rowStart_(0), rowEnd_(height), blockStart_(0) { fillOffsets(firstBlockDim, lastBlockDim); + matrix_.resize(height, variableColOffsets_.back()); assertInvariants(); } @@ -112,7 +112,7 @@ namespace gtsam { Index actualEndBlock = endBlock + blockStart_; if(startBlock != 0 || endBlock != 0) { checkBlock(actualStartBlock); - checkBlock(actualEndBlock); + assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); } const Index startCol = variableColOffsets_[actualStartBlock]; const Index rangeCols = variableColOffsets_[actualEndBlock] - startCol; @@ -125,7 +125,7 @@ namespace gtsam { Index actualEndBlock = endBlock + blockStart_; if(startBlock != 0 || endBlock != 0) { checkBlock(actualStartBlock); - checkBlock(actualEndBlock); + assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); } const Index startCol = variableColOffsets_[actualStartBlock]; const Index rangeCols = variableColOffsets_[actualEndBlock] - startCol; @@ -190,7 +190,7 @@ namespace gtsam { void checkBlock(DenseIndex block) const { 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()); }