From 049631e530b1bb2219375e906a752bd582b7d337 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Nov 2014 12:57:13 +0100 Subject: [PATCH] Avoid re-allocating vertical offsets --- .cproject | 106 +++++++++---------- gtsam/base/VerticalBlockMatrix.h | 39 ++++--- gtsam/base/tests/testVerticalBlockMatrix.cpp | 23 +++- 3 files changed, 91 insertions(+), 77 deletions(-) diff --git a/.cproject b/.cproject index ce5ac0778..a596e90bf 100644 --- a/.cproject +++ b/.cproject @@ -600,7 +600,6 @@ make - tests/testBayesTree.run true false @@ -608,7 +607,6 @@ make - testBinaryBayesNet.run true false @@ -656,7 +654,6 @@ make - testSymbolicBayesNet.run true false @@ -664,7 +661,6 @@ make - tests/testSymbolicFactor.run true false @@ -672,7 +668,6 @@ make - testSymbolicFactorGraph.run true false @@ -688,7 +683,6 @@ make - tests/testBayesTree true false @@ -1120,7 +1114,6 @@ make - testErrors.run true false @@ -1350,46 +1343,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1472,6 +1425,7 @@ make + testSimulated2DOriented.run true false @@ -1511,6 +1465,7 @@ make + testSimulated2D.run true false @@ -1518,6 +1473,7 @@ make + testSimulated3D.run true false @@ -1531,6 +1487,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1788,7 +1784,6 @@ cpack - -G DEB true false @@ -1796,7 +1791,6 @@ cpack - -G RPM true false @@ -1804,7 +1798,6 @@ cpack - -G TGZ true false @@ -1812,7 +1805,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2441,6 +2433,14 @@ true true + + make + -j5 + testVerticalBlockMatrix.run + true + true + true + make -j5 @@ -2579,7 +2579,6 @@ make - testGraph.run true false @@ -2587,7 +2586,6 @@ make - testJunctionTree.run true false @@ -2595,7 +2593,6 @@ make - testSymbolicBayesNetB.run true false @@ -3115,6 +3112,7 @@ make + tests/testGaussianISAM2 true false diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index c09cc7577..b075d73b3 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -65,9 +65,10 @@ namespace gtsam { /** Construct from a container of the sizes of each vertical block. */ template - VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) : - rowStart_(0), rowEnd_(height), blockStart_(0) - { + VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, + bool appendOneDimension = false) : + variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)), + rowStart_(0), rowEnd_(height), blockStart_(0) { fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); matrix_.resize(height, variableColOffsets_.back()); assertInvariants(); @@ -75,26 +76,28 @@ namespace gtsam { /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ template - VerticalBlockMatrix(const CONTAINER& dimensions, const Eigen::MatrixBase& matrix, bool appendOneDimension = false) : - matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) - { + VerticalBlockMatrix(const CONTAINER& dimensions, + const Eigen::MatrixBase& matrix, bool appendOneDimension = false) : + matrix_(matrix), variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)), + rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) { fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); - 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."); + 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."); assertInvariants(); } - /** - * Construct from iterator over the sizes of each vertical block. */ + /** Construct from iterator over the sizes of each vertical block. */ template - VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) : - rowStart_(0), rowEnd_(height), blockStart_(0) - { + VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, + DenseIndex height, bool appendOneDimension = false) : + variableColOffsets_((lastBlockDim-firstBlockDim) + (appendOneDimension ? 2 : 1)), + rowStart_(0), rowEnd_(height), blockStart_(0) { fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); matrix_.resize(height, variableColOffsets_.back()); 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 * the corresponding matrix view. In the destination VerticalBlockView, blockStart() and @@ -203,18 +206,12 @@ namespace gtsam { template void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) { - variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0)); variableColOffsets_[0] = 0; DenseIndex j=0; - for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { + for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim, ++j) variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; - ++ j; - } if(appendOneDimension) - { variableColOffsets_[j+1] = variableColOffsets_[j] + 1; - ++ j; - } } friend class SymmetricBlockMatrix; diff --git a/gtsam/base/tests/testVerticalBlockMatrix.cpp b/gtsam/base/tests/testVerticalBlockMatrix.cpp index fad23fa7d..c504752aa 100644 --- a/gtsam/base/tests/testVerticalBlockMatrix.cpp +++ b/gtsam/base/tests/testVerticalBlockMatrix.cpp @@ -24,9 +24,20 @@ using namespace std; using namespace gtsam; using boost::assign::list_of; +list L = list_of(3)(2)(1); +vector dimensions(L.begin(),L.end()); + //***************************************************************************** -TEST(VerticalBlockMatrix, constructor) { - VerticalBlockMatrix actual(list_of(3)(2)(1), +TEST(VerticalBlockMatrix, Constructor1) { + VerticalBlockMatrix actual(dimensions,6); + EXPECT_LONGS_EQUAL(6,actual.rows()); + EXPECT_LONGS_EQUAL(6,actual.cols()); + EXPECT_LONGS_EQUAL(3,actual.nBlocks()); +} + +//***************************************************************************** +TEST(VerticalBlockMatrix, Constructor2) { + VerticalBlockMatrix actual(dimensions, (Matrix(6, 6) << 1, 2, 3, 4, 5, 6, // 2, 8, 9, 10, 11, 12, // 3, 9, 15, 16, 17, 18, // @@ -38,6 +49,14 @@ TEST(VerticalBlockMatrix, constructor) { EXPECT_LONGS_EQUAL(3,actual.nBlocks()); } +//***************************************************************************** +TEST(VerticalBlockMatrix, Constructor3) { + VerticalBlockMatrix actual(dimensions.begin(),dimensions.end(),6); + EXPECT_LONGS_EQUAL(6,actual.rows()); + EXPECT_LONGS_EQUAL(6,actual.cols()); + EXPECT_LONGS_EQUAL(3,actual.nBlocks()); +} + //***************************************************************************** int main() { TestResult tr;