From 12e38a44e4f502abce324e886daef494c60b195d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Nov 2014 14:13:08 +0100 Subject: [PATCH 1/5] WriteableJacobianFactor will allow ExpressionFactor to write into the factor directly, (hopefull) eliminating huge overhead. --- .../nonlinear/tests/testExpressionFactor.cpp | 55 +++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index b0900a43b..b5476bee9 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -28,6 +28,9 @@ #include +#include +using boost::assign::list_of; + using namespace std; using namespace gtsam; @@ -420,6 +423,58 @@ TEST(ExpressionFactor, composeTernary) { EXPECT( assert_equal(expected, *jf,1e-9)); } +/* ************************************************************************* */ + +/** + * Special version of JacobianFactor that allows Jacobians to be written + */ +class WriteableJacobianFactor: public JacobianFactor { + + /** + * Constructor + * @param keys in some order + * @param diemnsions of the variables in same order + * @param m output dimension + * @param model noise model (default NULL) + */ + template + WriteableJacobianFactor(const KEYS& keys, const DIMENSIONS& dims, + DenseIndex m, const SharedDiagonal& model = SharedDiagonal()) { + + // Check noise model dimension + if (model && (DenseIndex) model->dim() != m) + throw InvalidNoiseModel(m, model->dim()); + + // copy the keys + keys_.resize(keys.size()); + std::copy(keys.begin(), keys.end(), keys_.begin()); + + // Check number of variables + if (dims.size() != keys_.size()) + throw std::invalid_argument( + "WriteableJacobianFactor: size of dimensions and keys do not agree."); + + Ab_ = VerticalBlockMatrix(dims.begin(), dims.end(), m, true); + Ab_.matrix().setZero(); + model_ = model; + } + friend class ExpressionFactorWriteableJacobianFactorTest; + template + friend class ExpressionFactor; +}; + +// Test Writeable JacobianFactor +TEST(ExpressionFactor, WriteableJacobianFactor) { + std::list keys = list_of(1)(2); + vector dimensions(2); + dimensions[0] = 6; + dimensions[1] = 3; + SharedDiagonal model; + WriteableJacobianFactor actual(keys, dimensions, 2, model); + JacobianFactor expected(1, zeros(2, 6), 2, zeros(2, 3), zero(2)); + EXPECT( assert_equal(expected, *(JacobianFactor*)(&actual),1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 7debde75184d3215e6552da48138dce173611553 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Nov 2014 15:12:06 +0100 Subject: [PATCH 2/5] Moved to ExpressionFactor that now uses it - timing seems worse ? --- gtsam_unstable/nonlinear/ExpressionFactor.h | 85 ++++++++++++++----- .../nonlinear/tests/testExpressionFactor.cpp | 39 --------- 2 files changed, 62 insertions(+), 62 deletions(-) diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index c9f3fae86..e94e2bec4 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -24,8 +24,57 @@ #include #include +class ExpressionFactorWriteableJacobianFactorTest; + namespace gtsam { +/** + * Special version of JacobianFactor that allows Jacobians to be written + * Eliminates a large proportion of overhead + * Note all ExpressionFactor are friends, not for general consumption. + */ +class WriteableJacobianFactor: public JacobianFactor { + +public: + + /** + * Constructor + * @param keys in some order + * @param diemnsions of the variables in same order + * @param m output dimension + * @param model noise model (default NULL) + */ + template + WriteableJacobianFactor(const KEYS& keys, const DIMENSIONS& dims, + DenseIndex m, const SharedDiagonal& model = SharedDiagonal()) { + + // Check noise model dimension + if (model && (DenseIndex) model->dim() != m) + throw InvalidNoiseModel(m, model->dim()); + + // copy the keys + keys_.resize(keys.size()); + std::copy(keys.begin(), keys.end(), keys_.begin()); + + // Check number of variables + if (dims.size() != keys_.size()) + throw std::invalid_argument( + "WriteableJacobianFactor: size of dimensions and keys do not agree."); + + Ab_ = VerticalBlockMatrix(dims.begin(), dims.end(), m, true); + Ab_.matrix().setZero(); + model_ = model; + } + + VerticalBlockMatrix& Ab() { + return Ab_; + } + +// friend class ::ExpressionFactorWriteableJacobianFactorTest; +// template +// friend class ExpressionFactor; +}; + /** * Factor that supports arbitrary expressions via AD */ @@ -106,42 +155,32 @@ public: virtual boost::shared_ptr linearize(const Values& x) const { - // This method has been heavily optimized for maximum performance. - // We allocate a VerticalBlockMatrix on the stack first, and then create - // Eigen::Block views on this piece of memory which is then passed - // to [expression_.value] below, which writes directly into Ab_. + // Create noise model + SharedDiagonal model; + noiseModel::Constrained::shared_ptr constrained = // + boost::dynamic_pointer_cast(this->noiseModel_); + if (constrained) + model = constrained->unit(); - // Another malloc saved by creating a Matrix on the stack - double memory[Dim * augmentedCols_]; - Eigen::Map > // - matrix(memory, Dim, augmentedCols_); - matrix.setZero(); // zero out - - // Construct block matrix, is of right size but un-initialized - VerticalBlockMatrix Ab(dimensions_, matrix, true); + // Create a writeable JacobianFactor in advance + boost::shared_ptr factor = boost::make_shared< + WriteableJacobianFactor>(keys_, dimensions_, + traits::dimension::value, model); // Create blocks into Ab_ to be passed to expression_ JacobianMap blocks; blocks.reserve(size()); for (DenseIndex i = 0; i < size(); i++) - blocks.push_back(std::make_pair(keys_[i], Ab(i))); + blocks.push_back(std::make_pair(keys_[i], factor->Ab()(i))); // Evaluate error to get Jacobians and RHS vector b T value = expression_.value(x, blocks); // <<< Reverse AD happens here ! - Ab(size()).col(0) = -measurement_.localCoordinates(value); + factor->Ab()(size()).col(0) = -measurement_.localCoordinates(value); // Whiten the corresponding system now // TODO ! this->noiseModel_->WhitenSystem(Ab); - // TODO pass unwhitened + noise model to Gaussian factor - // For now, only linearized constrained factors have noise model at linear level!!! - noiseModel::Constrained::shared_ptr constrained = // - boost::dynamic_pointer_cast(this->noiseModel_); - if (constrained) { - return boost::make_shared(this->keys(), Ab, - constrained->unit()); - } else - return boost::make_shared(this->keys(), Ab); + return factor; } }; // ExpressionFactor diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index b5476bee9..f9a4bc532 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -424,45 +424,6 @@ TEST(ExpressionFactor, composeTernary) { } /* ************************************************************************* */ - -/** - * Special version of JacobianFactor that allows Jacobians to be written - */ -class WriteableJacobianFactor: public JacobianFactor { - - /** - * Constructor - * @param keys in some order - * @param diemnsions of the variables in same order - * @param m output dimension - * @param model noise model (default NULL) - */ - template - WriteableJacobianFactor(const KEYS& keys, const DIMENSIONS& dims, - DenseIndex m, const SharedDiagonal& model = SharedDiagonal()) { - - // Check noise model dimension - if (model && (DenseIndex) model->dim() != m) - throw InvalidNoiseModel(m, model->dim()); - - // copy the keys - keys_.resize(keys.size()); - std::copy(keys.begin(), keys.end(), keys_.begin()); - - // Check number of variables - if (dims.size() != keys_.size()) - throw std::invalid_argument( - "WriteableJacobianFactor: size of dimensions and keys do not agree."); - - Ab_ = VerticalBlockMatrix(dims.begin(), dims.end(), m, true); - Ab_.matrix().setZero(); - model_ = model; - } - friend class ExpressionFactorWriteableJacobianFactorTest; - template - friend class ExpressionFactor; -}; - // Test Writeable JacobianFactor TEST(ExpressionFactor, WriteableJacobianFactor) { std::list keys = list_of(1)(2); From cb69f2cb8285b78d4f2b382a5fca5449f909891a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Nov 2014 11:40:48 +0100 Subject: [PATCH 3/5] Fastest linearize so far. Putting 'unsafe' constructor in JacobianFactor itself makes a *huge* difference. --- gtsam/linear/JacobianFactor.h | 19 +++++++ gtsam_unstable/nonlinear/ExpressionFactor.h | 55 +------------------ .../nonlinear/tests/testExpressionFactor.cpp | 4 +- 3 files changed, 24 insertions(+), 54 deletions(-) diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index b90012822..d98596a9f 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -140,6 +140,25 @@ namespace gtsam { JacobianFactor( const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); + /** + * Constructor + * @param keys in some order + * @param diemnsions of the variables in same order + * @param m output dimension + * @param model noise model (default NULL) + */ + template + JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m, + const SharedDiagonal& model = SharedDiagonal()) : + Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) { + Ab_.matrix().setZero(); + } + + /// Direct access to VerticalBlockMatrix + VerticalBlockMatrix& Ab() { + return Ab_; + } + /** * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots * structure computed for \c graph is already available, providing it will reduce the amount of diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index aaf10dc98..0394fbe5c 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -24,57 +24,8 @@ #include #include -class ExpressionFactorWriteableJacobianFactorTest; - namespace gtsam { -/** - * Special version of JacobianFactor that allows Jacobians to be written - * Eliminates a large proportion of overhead - * Note all ExpressionFactor are friends, not for general consumption. - */ -class WriteableJacobianFactor: public JacobianFactor { - -public: - - /** - * Constructor - * @param keys in some order - * @param diemnsions of the variables in same order - * @param m output dimension - * @param model noise model (default NULL) - */ - template - WriteableJacobianFactor(const KEYS& keys, const DIMENSIONS& dims, - DenseIndex m, const SharedDiagonal& model = SharedDiagonal()) { - - // Check noise model dimension - if (model && (DenseIndex) model->dim() != m) - throw InvalidNoiseModel(m, model->dim()); - - // copy the keys - keys_.resize(keys.size()); - std::copy(keys.begin(), keys.end(), keys_.begin()); - - // Check number of variables - if (dims.size() != keys_.size()) - throw std::invalid_argument( - "WriteableJacobianFactor: size of dimensions and keys do not agree."); - - Ab_ = VerticalBlockMatrix(dims.begin(), dims.end(), m, true); - Ab_.matrix().setZero(); - model_ = model; - } - - VerticalBlockMatrix& Ab() { - return Ab_; - } - -// friend class ::ExpressionFactorWriteableJacobianFactorTest; -// template -// friend class ExpressionFactor; -}; - /** * Factor that supports arbitrary expressions via AD */ @@ -164,9 +115,9 @@ public: model = constrained->unit(); // Create a writeable JacobianFactor in advance - boost::shared_ptr factor = boost::make_shared< - WriteableJacobianFactor>(keys_, dimensions_, - traits::dimension::value, model); + boost::shared_ptr factor( + new JacobianFactor(keys_, dimensions_, traits::dimension::value, + model)); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ JacobianMap map(keys_, factor->Ab()); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index f9a4bc532..08496e5ff 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -431,9 +431,9 @@ TEST(ExpressionFactor, WriteableJacobianFactor) { dimensions[0] = 6; dimensions[1] = 3; SharedDiagonal model; - WriteableJacobianFactor actual(keys, dimensions, 2, model); + JacobianFactor actual(keys, dimensions, 2, model); JacobianFactor expected(1, zeros(2, 6), 2, zeros(2, 3), zero(2)); - EXPECT( assert_equal(expected, *(JacobianFactor*)(&actual),1e-9)); + EXPECT( assert_equal(expected, actual,1e-9)); } /* ************************************************************************* */ From b9e3c3b11609860cc031929503dc6173e7092e23 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Nov 2014 12:01:52 +0100 Subject: [PATCH 4/5] Made unsafe constructor private, but made ExpressionFactor a friend. --- gtsam/linear/JacobianFactor.h | 46 +++++++++---------- gtsam_unstable/nonlinear/ExpressionFactor.h | 18 ++++---- .../nonlinear/tests/testExpressionFactor.cpp | 13 ------ 3 files changed, 32 insertions(+), 45 deletions(-) diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index d98596a9f..d1dc7625c 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -82,20 +82,22 @@ namespace gtsam { class GTSAM_EXPORT JacobianFactor : public GaussianFactor { public: + typedef JacobianFactor This; ///< Typedef to this class typedef GaussianFactor Base; ///< Typedef to base class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - protected: - VerticalBlockMatrix Ab_; // the block view of the full matrix - noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix - - public: typedef VerticalBlockMatrix::Block ABlock; typedef VerticalBlockMatrix::constBlock constABlock; typedef ABlock::ColXpr BVector; typedef constABlock::ConstColXpr constBVector; + protected: + + VerticalBlockMatrix Ab_; // the block view of the full matrix + noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix + + public: /** Convert from other GaussianFactor */ explicit JacobianFactor(const GaussianFactor& gf); @@ -140,25 +142,6 @@ namespace gtsam { JacobianFactor( const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); - /** - * Constructor - * @param keys in some order - * @param diemnsions of the variables in same order - * @param m output dimension - * @param model noise model (default NULL) - */ - template - JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m, - const SharedDiagonal& model = SharedDiagonal()) : - Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) { - Ab_.matrix().setZero(); - } - - /// Direct access to VerticalBlockMatrix - VerticalBlockMatrix& Ab() { - return Ab_; - } - /** * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots * structure computed for \c graph is already available, providing it will reduce the amount of @@ -347,6 +330,21 @@ namespace gtsam { private: + /** Unsafe Constructor that creates an uninitialized Jacobian of right size + * @param keys in some order + * @param diemnsions of the variables in same order + * @param m output dimension + * @param model noise model (default NULL) + */ + template + JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m, + const SharedDiagonal& model = SharedDiagonal()) : + Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) { + } + + // be very selective on who can access these private methods: + template friend class ExpressionFactor; + /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 0394fbe5c..1d5d0cb12 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -107,24 +107,26 @@ public: virtual boost::shared_ptr linearize(const Values& x) const { - // Create noise model - SharedDiagonal model; + // Check whether noise model is constrained or not noiseModel::Constrained::shared_ptr constrained = // boost::dynamic_pointer_cast(this->noiseModel_); - if (constrained) - model = constrained->unit(); // Create a writeable JacobianFactor in advance boost::shared_ptr factor( - new JacobianFactor(keys_, dimensions_, traits::dimension::value, - model)); + constrained ? new JacobianFactor(keys_, dimensions_, Dim, + constrained->unit()) : + new JacobianFactor(keys_, dimensions_, Dim)); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ - JacobianMap map(keys_, factor->Ab()); + VerticalBlockMatrix& Ab = factor->matrixObject(); + JacobianMap map(keys_, Ab); + + // Zero out Jacobian so we can simply add to it + Ab.matrix().setZero(); // Evaluate error to get Jacobians and RHS vector b T value = expression_.value(x, map); // <<< Reverse AD happens here ! - factor->Ab()(size()).col(0) = -measurement_.localCoordinates(value); + Ab(size()).col(0) = -measurement_.localCoordinates(value); // Whiten the corresponding system now // TODO ! this->noiseModel_->WhitenSystem(Ab); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 08496e5ff..23c2fa1ce 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -423,19 +423,6 @@ TEST(ExpressionFactor, composeTernary) { EXPECT( assert_equal(expected, *jf,1e-9)); } -/* ************************************************************************* */ -// Test Writeable JacobianFactor -TEST(ExpressionFactor, WriteableJacobianFactor) { - std::list keys = list_of(1)(2); - vector dimensions(2); - dimensions[0] = 6; - dimensions[1] = 3; - SharedDiagonal model; - JacobianFactor actual(keys, dimensions, 2, model); - JacobianFactor expected(1, zeros(2, 6), 2, zeros(2, 3), zero(2)); - EXPECT( assert_equal(expected, actual,1e-9)); -} - /* ************************************************************************* */ int main() { TestResult tr; From 049631e530b1bb2219375e906a752bd582b7d337 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Nov 2014 12:57:13 +0100 Subject: [PATCH 5/5] 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;