Fixed problems in block matrices

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(),
+    for(size_t i = 0; i < result.variableColOffsets_.size(); ++i)
-      result.variableColOffsets_.begin());
+      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(),
+    for(size_t i = 0; i < result.variableColOffsets_.size(); ++i)
-      result.variableColOffsets_.begin());
+      result.variableColOffsets_[i] =
+      other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_];
     result.matrix_.resize(other.cols(), other.cols());
     result.assertInvariants();
     return result;

gtsam/base/SymmetricBlockMatrix.h

@@ -59,7 +59,7 @@ namespace gtsam {
 
     /** Construct from a container of the sizes of each block. */
     template<typename CONTAINER>
-    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<typename CONTAINER>
-    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());

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);
+    result.variableColOffsets_.resize(other.nBlocks() + 1);
-    std::copy(rhs.variableColOffsets_.begin() + rhs.blockStart_, rhs.variableColOffsets_.end(),
+    for(size_t i = 0; i < result.variableColOffsets_.size(); ++i)
-      result.variableColOffsets_.begin());
+      result.variableColOffsets_[i] =
-    result.matrix_.resize(rhs.rows(), result.variableColOffsets_.back());
+      other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_];
-    result.rowEnd_ = rhs.rows();
+    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);
+    result.variableColOffsets_.resize(other.nBlocks() + 1);
-    std::copy(rhs.variableColOffsets_.begin() + rhs.blockStart_, rhs.variableColOffsets_.end(),
+    for(size_t i = 0; i < result.variableColOffsets_.size(); ++i)
-      result.variableColOffsets_.begin());
+      result.variableColOffsets_[i] =
+      other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_];
     result.matrix_.resize(height, result.variableColOffsets_.back());
     result.rowEnd_ = height;
     result.assertInvariants();

gtsam/base/VerticalBlockMatrix.h

@@ -64,7 +64,7 @@ namespace gtsam {
 
     /** Construct from a container of the sizes of each vertical block. */
     template<typename CONTAINER>
-    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<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)
     {
       fillOffsets(dimensions.begin(), dimensions.end());