diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 378e91144..f6d4fa9c2 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -256,11 +256,16 @@ namespace gtsam { full().triangularView() = xpr.template triangularView(); } - /// Set the entire active matrix zero. + /// Set the entire *active* matrix zero. void setZero() { full().triangularView().setZero(); } + /// Set entire matrix zero. + void setAllZero() { + matrix_.setZero(); + } + /// Negate the entire active matrix. void negate(); diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 0ff8a8ae2..741d0e4bf 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -57,13 +57,46 @@ namespace gtsam { DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment. public: - /** Construct an empty VerticalBlockMatrix */ - VerticalBlockMatrix() : - rowStart_(0), rowEnd_(0), blockStart_(0) - { + VerticalBlockMatrix() : rowStart_(0), rowEnd_(0), blockStart_(0) { variableColOffsets_.push_back(0); - assertInvariants(); + } + + // Destructor + ~VerticalBlockMatrix() = default; + + // Copy constructor (default) + VerticalBlockMatrix(const VerticalBlockMatrix& other) = default; + + // Copy assignment operator (default) + VerticalBlockMatrix& operator=(const VerticalBlockMatrix& other) = default; + + // Move constructor + VerticalBlockMatrix(VerticalBlockMatrix&& other) noexcept + : matrix_(std::move(other.matrix_)), + variableColOffsets_(std::move(other.variableColOffsets_)), + rowStart_(other.rowStart_), + rowEnd_(other.rowEnd_), + blockStart_(other.blockStart_) { + other.rowStart_ = 0; + other.rowEnd_ = 0; + other.blockStart_ = 0; + } + + // Move assignment operator + VerticalBlockMatrix& operator=(VerticalBlockMatrix&& other) noexcept { + if (this != &other) { + matrix_ = std::move(other.matrix_); + variableColOffsets_ = std::move(other.variableColOffsets_); + rowStart_ = other.rowStart_; + rowEnd_ = other.rowEnd_; + blockStart_ = other.blockStart_; + + other.rowStart_ = 0; + other.rowEnd_ = 0; + other.blockStart_ = 0; + } + return *this; } /** Construct from a container of the sizes of each vertical block. */ diff --git a/gtsam/linear/GaussianConditional-inl.h b/gtsam/linear/GaussianConditional-inl.h index fe5b1e0d7..2756b690d 100644 --- a/gtsam/linear/GaussianConditional-inl.h +++ b/gtsam/linear/GaussianConditional-inl.h @@ -33,4 +33,10 @@ namespace gtsam { const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas) : BaseFactor(keys, augmentedMatrix, sigmas), BaseConditional(nrFrontals) {} + /* ************************************************************************* */ + template + GaussianConditional::GaussianConditional( + const KEYS& keys, size_t nrFrontals, VerticalBlockMatrix&& augmentedMatrix, const SharedDiagonal& sigmas) : + BaseFactor(keys, std::move(augmentedMatrix), sigmas), BaseConditional(nrFrontals) {} + } // gtsam diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 14b1ce87f..d71119d6a 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -75,14 +75,35 @@ namespace gtsam { size_t nrFrontals, const Vector& d, const SharedDiagonal& sigmas = SharedDiagonal()); - /** Constructor with arbitrary number keys, and where the augmented matrix is given all together - * instead of in block terms. Note that only the active view of the provided augmented matrix - * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed - * factor. */ - template - GaussianConditional( - const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix, - const SharedDiagonal& sigmas = SharedDiagonal()); + /** + * @brief Constructor with an arbitrary number of keys, where the augmented matrix + * is given all together instead of in block terms. + * + * @tparam KEYS Type of the keys container. + * @param keys Container of keys. + * @param nrFrontals Number of frontal variables. + * @param augmentedMatrix The augmented matrix containing the coefficients. + * @param sigmas Optional noise model (default is an empty SharedDiagonal). + */ + template + GaussianConditional(const KEYS& keys, size_t nrFrontals, + const VerticalBlockMatrix& augmentedMatrix, + const SharedDiagonal& sigmas = SharedDiagonal()); + + /** + * @brief Constructor with an arbitrary number of keys, where the augmented matrix + * is given all together instead of in block terms, using move semantics for efficiency. + * + * @tparam KEYS Type of the keys container. + * @param keys Container of keys. + * @param nrFrontals Number of frontal variables. + * @param augmentedMatrix The augmented matrix containing the coefficients (moved). + * @param sigmas Optional noise model (default is an empty SharedDiagonal). + */ + template + GaussianConditional(const KEYS& keys, size_t nrFrontals, + VerticalBlockMatrix&& augmentedMatrix, + const SharedDiagonal& sigmas = SharedDiagonal()); /// Construct from mean `mu` and standard deviation `sigma`. static GaussianConditional FromMeanAndStddev(Key key, const Vector& mu, diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 1172dc281..0a75cc336 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -245,7 +245,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, // Form A' * A gttic(update); - info_.setZero(); + info_.setAllZero(); for(const auto& factor: factors) if (factor) factor->updateHessian(keys_, &info_); @@ -348,28 +348,26 @@ double HessianFactor::error(const VectorValues& c) const { /* ************************************************************************* */ void HessianFactor::updateHessian(const KeyVector& infoKeys, SymmetricBlockMatrix* info) const { - gttic(updateHessian_HessianFactor); assert(info); - // Apply updates to the upper triangle - DenseIndex nrVariablesInThisFactor = size(), nrBlocksInInfo = info->nBlocks() - 1; + gttic(updateHessian_HessianFactor); + const DenseIndex nrVariablesInThisFactor = size(); + vector slots(nrVariablesInThisFactor + 1); + for (DenseIndex j = 0; j < nrVariablesInThisFactor; ++j) + slots[j] = Slot(infoKeys, keys_[j]); + slots[nrVariablesInThisFactor] = info->nBlocks() - 1; + + // Apply updates to the upper triangle // Loop over this factor's blocks with indices (i,j) // For every block (i,j), we determine the block (I,J) in info. for (DenseIndex j = 0; j <= nrVariablesInThisFactor; ++j) { - const bool rhs = (j == nrVariablesInThisFactor); - const DenseIndex J = rhs ? nrBlocksInInfo : Slot(infoKeys, keys_[j]); - slots[j] = J; - for (DenseIndex i = 0; i <= j; ++i) { - const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid. - - if (i == j) { - assert(I == J); - info->updateDiagonalBlock(I, info_.diagonalBlock(i)); - } else { - assert(i < j); - assert(I != J); - info->updateOffDiagonalBlock(I, J, info_.aboveDiagonalBlock(i, j)); - } + const DenseIndex J = slots[j]; + info->updateDiagonalBlock(J, info_.diagonalBlock(j)); + for (DenseIndex i = 0; i < j; ++i) { + const DenseIndex I = slots[i]; + assert(i < j); + assert(I != J); + info->updateOffDiagonalBlock(I, J, info_.aboveDiagonalBlock(i, j)); } } } @@ -470,7 +468,7 @@ std::shared_ptr HessianFactor::eliminateCholesky(const Orde // TODO(frank): pre-allocate GaussianConditional and write into it const VerticalBlockMatrix Ab = info_.split(nFrontals); - conditional = std::make_shared(keys_, nFrontals, Ab); + conditional = std::make_shared(keys_, nFrontals, std::move(Ab)); // Erase the eliminated keys in this factor keys_.erase(begin(), begin() + nFrontals); diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index 6c4cb969a..aee0b7563 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -23,36 +23,28 @@ namespace gtsam { /* ************************************************************************* */ - template - JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, const SharedDiagonal& model) - { + template + JacobianFactor::JacobianFactor(const TERMS& terms, const Vector& b, + const SharedDiagonal& model) { fillTerms(terms, b, model); } /* ************************************************************************* */ - template - JacobianFactor::JacobianFactor( - const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) : - Base(keys), Ab_(augmentedMatrix) - { - // Check noise model dimension - if(model && (DenseIndex)model->dim() != augmentedMatrix.rows()) - throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim()); + template + JacobianFactor::JacobianFactor(const KEYS& keys, + const VerticalBlockMatrix& augmentedMatrix, + const SharedDiagonal& model) + : Base(keys), Ab_(augmentedMatrix), model_(model) { + checkAb(model, augmentedMatrix); + } - // Check number of variables - if((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1) - throw std::invalid_argument( - "Error in JacobianFactor constructor input. Number of provided keys plus\n" - "one for the RHS vector must equal the number of provided matrix blocks."); - - // Check RHS dimension - if(augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1) - throw std::invalid_argument( - "Error in JacobianFactor constructor input. The last provided matrix block\n" - "must be the RHS vector, but the last provided block had more than one column."); - - // Take noise model - model_ = model; + /* ************************************************************************* */ + template + JacobianFactor::JacobianFactor(const KEYS& keys, + VerticalBlockMatrix&& augmentedMatrix, + const SharedDiagonal& model) + : Base(keys), Ab_(std::move(augmentedMatrix)), model_(model) { + checkAb(model, Ab_); } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 51d513e33..1802475eb 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -112,6 +112,28 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor) } } + /* ************************************************************************* */ +void JacobianFactor::checkAb(const SharedDiagonal& model, + const VerticalBlockMatrix& augmentedMatrix) const { + // Check noise model dimension + if (model && (DenseIndex)model->dim() != augmentedMatrix.rows()) + throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim()); + + // Check number of variables + if ((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1) + throw std::invalid_argument( + "Error in JacobianFactor constructor input. Number of provided keys " + "plus one for the RHS vector must equal the number of provided " + "matrix blocks."); + + // Check RHS dimension + if (augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1) + throw std::invalid_argument( + "Error in JacobianFactor constructor input. The last provided " + "matrix block must be the RHS vector, but the last provided block " + "had more than one column."); +} + /* ************************************************************************* */ // Helper functions for combine constructor namespace { @@ -580,16 +602,19 @@ void JacobianFactor::updateHessian(const KeyVector& infoKeys, // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1; + // Pre-calculate slots + vector slots(n + 1); + for (DenseIndex j = 0; j < n; ++j) slots[j] = Slot(infoKeys, keys_[j]); + slots[n] = N; + // Apply updates to the upper triangle // Loop over blocks of A, including RHS with j==n - vector slots(n+1); for (DenseIndex j = 0; j <= n; ++j) { Eigen::Block Ab_j = Ab_(j); - const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]); - slots[j] = J; + const DenseIndex J = slots[j]; // Fill off-diagonal blocks with Ai'*Aj for (DenseIndex i = 0; i < j; ++i) { - const DenseIndex I = slots[i]; // because iupdateOffDiagonalBlock(I, J, Ab_(i).transpose() * Ab_j); } // Fill diagonal block with Aj'*Aj diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index a9933374f..33f7183a6 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -145,13 +145,17 @@ namespace gtsam { template JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal()); - /** Constructor with arbitrary number keys, and where the augmented matrix is given all together - * instead of in block terms. Note that only the active view of the provided augmented matrix - * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed - * factor. */ - template - JacobianFactor( - const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); + /** Constructor with arbitrary number keys, and where the augmented matrix + * is given all together instead of in block terms. + */ + template + JacobianFactor(const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, + const SharedDiagonal& sigmas = SharedDiagonal()); + + /** Construct with an rvalue VerticalBlockMatrix, to allow std::move. */ + template + JacobianFactor(const KEYS& keys, VerticalBlockMatrix&& augmentedMatrix, + const SharedDiagonal& model); /** * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots @@ -398,7 +402,11 @@ namespace gtsam { template void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel); - private: + /// Common code between VerticalBlockMatrix constructors + void checkAb(const SharedDiagonal& model, + const VerticalBlockMatrix& augmentedMatrix) const; + + private: /** * Helper function for public constructors: diff --git a/timing/timeBatch.cpp b/timing/timeBatch.cpp index f59039fa7..ec7222e61 100644 --- a/timing/timeBatch.cpp +++ b/timing/timeBatch.cpp @@ -29,18 +29,16 @@ int main(int argc, char *argv[]) { cout << "Loading data..." << endl; string datasetFile = findExampleDataFile("w10000"); - std::pair data = - load2D(datasetFile); - - NonlinearFactorGraph graph = *data.first; - Values initial = *data.second; + auto [graph, initial] = load2D(datasetFile); + graph->addPrior(0, initial->at(0), noiseModel::Unit::Create(3)); cout << "Optimizing..." << endl; gttic_(Create_optimizer); - LevenbergMarquardtOptimizer optimizer(graph, initial); + LevenbergMarquardtOptimizer optimizer(*graph, *initial); gttoc_(Create_optimizer); tictoc_print_(); + tictoc_reset_(); double lastError = optimizer.error(); do { gttic_(Iterate_optimizer); @@ -53,19 +51,19 @@ int main(int argc, char *argv[]) { } while(!checkConvergence(optimizer.params().relativeErrorTol, optimizer.params().absoluteErrorTol, optimizer.params().errorTol, lastError, optimizer.error(), optimizer.params().verbosity)); + tictoc_reset_(); // Compute marginals - Marginals marginals(graph, optimizer.values()); - int i=0; - for(Key key: initial.keys()) { + gttic_(ConstructMarginals); + Marginals marginals(*graph, optimizer.values()); + gttoc_(ConstructMarginals); + for(Key key: initial->keys()) { gttic_(marginalInformation); Matrix info = marginals.marginalInformation(key); gttoc_(marginalInformation); tictoc_finishedIteration_(); - if(i % 1000 == 0) - tictoc_print_(); - ++i; } + tictoc_print_(); } catch(std::exception& e) { cout << e.what() << endl;