diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index 02f8b3fc0..beabc7755 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -26,8 +26,9 @@ namespace gtsam { { SymmetricBlockMatrix result; result.variableColOffsets_.resize(other.nBlocks() + 1); - std::copy(other.variableColOffsets_.begin() + other.blockStart_, other.variableColOffsets_.end(), - result.variableColOffsets_.begin()); + 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; @@ -38,8 +39,9 @@ namespace gtsam { { SymmetricBlockMatrix result; result.variableColOffsets_.resize(other.nBlocks() + 1); - std::copy(other.variableColOffsets_.begin() + other.blockStart_, other.variableColOffsets_.end(), - result.variableColOffsets_.begin()); + 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; diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index fd9ec42f1..8b4db01eb 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -59,7 +59,7 @@ namespace gtsam { /** Construct from a container of the sizes of each block. */ template - SymmetricBlockMatrix(const CONTAINER dimensions) : + SymmetricBlockMatrix(const CONTAINER& dimensions) : blockStart_(0) { fillOffsets(dimensions.begin(), dimensions.end()); @@ -80,7 +80,7 @@ namespace gtsam { /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ template - SymmetricBlockMatrix(const CONTAINER dimensions, const Matrix& matrix) : + SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) : matrix_(matrix), blockStart_(0) { fillOffsets(dimensions.begin(), dimensions.end()); @@ -147,8 +147,6 @@ namespace gtsam { DenseIndex i_actualEndBlock = i_endBlock + blockStart_; DenseIndex j_actualStartBlock = j_startBlock + blockStart_; 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()); diff --git a/gtsam/base/VerticalBlockMatrix.cpp b/gtsam/base/VerticalBlockMatrix.cpp index 8d9598bf4..0361264fe 100644 --- a/gtsam/base/VerticalBlockMatrix.cpp +++ b/gtsam/base/VerticalBlockMatrix.cpp @@ -22,25 +22,27 @@ namespace gtsam { /* ************************************************************************* */ - VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& rhs) + VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& other) { VerticalBlockMatrix result; - result.variableColOffsets_.resize(rhs.nBlocks() + 1); - std::copy(rhs.variableColOffsets_.begin() + rhs.blockStart_, rhs.variableColOffsets_.end(), - result.variableColOffsets_.begin()); - result.matrix_.resize(rhs.rows(), result.variableColOffsets_.back()); - result.rowEnd_ = rhs.rows(); + 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& rhs, DenseIndex height) + VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const SymmetricBlockMatrix& other, DenseIndex height) { VerticalBlockMatrix result; - result.variableColOffsets_.resize(rhs.nBlocks() + 1); - std::copy(rhs.variableColOffsets_.begin() + rhs.blockStart_, rhs.variableColOffsets_.end(), - result.variableColOffsets_.begin()); + 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(); diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 502d411d8..7677e817e 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -64,7 +64,7 @@ namespace gtsam { /** Construct from a container of the sizes of each vertical block. */ template - VerticalBlockMatrix(const CONTAINER dimensions, DenseIndex height) : + VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height) : rowStart_(0), rowEnd_(height), blockStart_(0) { fillOffsets(dimensions.begin(), dimensions.end()); @@ -74,7 +74,7 @@ namespace gtsam { /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ template - VerticalBlockMatrix(const CONTAINER dimensions, const Matrix& matrix) : + VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) : matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) { fillOffsets(dimensions.begin(), dimensions.end());