From 39ffe3ac32f7143de468e6a0f241243a21b4186c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 10 Jun 2015 15:53:43 -0400 Subject: [PATCH] Made updateATA a virtual method for a small saving in CPU, but more importantly to allow for custom Jacobian or HessianFactors... --- gtsam/linear/GaussianFactor.h | 10 +++ gtsam/linear/HessianFactor.cpp | 115 ++++++-------------------------- gtsam/linear/HessianFactor.h | 13 +--- gtsam/linear/JacobianFactor.cpp | 43 ++++++++++++ gtsam/linear/JacobianFactor.h | 7 ++ 5 files changed, 85 insertions(+), 103 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 6a7d91bc9..bb4b20e58 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -28,6 +28,8 @@ namespace gtsam { // Forward declarations class VectorValues; + class Scatter; + class SymmetricBlockMatrix; /** * An abstract virtual base class for JacobianFactor and HessianFactor. A GaussianFactor has a @@ -119,6 +121,14 @@ namespace gtsam { */ virtual GaussianFactor::shared_ptr negate() const = 0; + /** Update an information matrix by adding the information corresponding to this factor + * (used internally during elimination). + * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param info The information matrix to be updated + */ + virtual void updateATA(const Scatter& scatter, + SymmetricBlockMatrix* info) const = 0; + /// y += alpha * A'*A*x virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 6df4e7337..9dbc2b52d 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -75,7 +75,7 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, const JacobianFactor* asJacobian = dynamic_cast(factor.get()); if (!asJacobian || asJacobian->cols() > 1) - this->insert( + insert( make_pair(*variable, SlotEntry(none, factor->getDim(variable)))); } } @@ -296,16 +296,8 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, // Form A' * A gttic(update); BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) - { - if(factor) { - if(const HessianFactor* hessian = dynamic_cast(factor.get())) - updateATA(*hessian, *scatter); - else if(const JacobianFactor* jacobian = dynamic_cast(factor.get())) - updateATA(*jacobian, *scatter); - else - throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); - } - } + if(factor) + factor->updateATA(*scatter, &info_); gttoc(update); } @@ -313,8 +305,8 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) const { cout << s << "\n"; cout << " keys: "; - for(const_iterator key=this->begin(); key!=this->end(); ++key) - cout << formatter(*key) << "(" << this->getDim(key) << ") "; + for(const_iterator key=begin(); key!=end(); ++key) + cout << formatter(*key) << "(" << getDim(key) << ") "; cout << "\n"; gtsam::print(Matrix(info_.full().selfadjointView()), "Augmented information matrix: "); } @@ -326,7 +318,7 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { else { if(!Factor::equals(lf, tol)) return false; - Matrix thisMatrix = this->info_.full().selfadjointView(); + Matrix thisMatrix = info_.full().selfadjointView(); thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; Matrix rhsMatrix = static_cast(lf).info_.full().selfadjointView(); rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; @@ -343,7 +335,7 @@ Matrix HessianFactor::augmentedInformation() const /* ************************************************************************* */ Matrix HessianFactor::information() const { - return info_.range(0, this->size(), 0, this->size()).selfadjointView(); + return info_.range(0, size(), 0, size()).selfadjointView(); } /* ************************************************************************* */ @@ -396,97 +388,34 @@ double HessianFactor::error(const VectorValues& c) const { double xtg = 0, xGx = 0; // extract the relevant subset of the VectorValues // NOTE may not be as efficient - const Vector x = c.vector(this->keys()); + const Vector x = c.vector(keys()); xtg = x.dot(linearTerm()); - xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; + xGx = x.transpose() * info_.range(0, size(), 0, size()).selfadjointView() * x; return 0.5 * (f - 2.0 * xtg + xGx); } /* ************************************************************************* */ -void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) -{ +void HessianFactor::updateATA(const Scatter& scatter, + SymmetricBlockMatrix* info) const { gttic(updateATA_HessianFactor); - // This function updates 'combined' with the information in 'update'. 'scatter' maps variables in - // the update factor to slots in the combined factor. + // N is number of variables in information matrix, n in HessianFactor + DenseIndex N = info->nBlocks() - 1, n = size(); // First build an array of slots - gttic(slots); - FastVector slots(update.size()); + FastVector slots(n + 1); DenseIndex slot = 0; - BOOST_FOREACH(Key j, update) { - slots[slot] = scatter.at(j).slot; - ++ slot; - } - gttoc(slots); + BOOST_FOREACH (Key key, *this) + slots[slot++] = scatter.at(key).slot; + slots[n] = N; // Apply updates to the upper triangle - gttic(update); - size_t nrInfoBlocks = this->info_.nBlocks(); - for(DenseIndex j2=0; j2 0) { - gttic(whiten); - // Whiten the factor if it has a noise model - boost::optional _whitenedFactor; - const JacobianFactor* whitenedFactor = &update; - const SharedDiagonal& model = update.get_model(); - if (model && !model->isUnit()) { - if (model->isConstrained()) - throw invalid_argument( - "Cannot update HessianFactor from JacobianFactor with constrained noise model"); - _whitenedFactor = update.whiten(); - whitenedFactor = &(*_whitenedFactor); - } - gttoc(whiten); - - // A is the whitened Jacobian matrix A, and we are going to perform I += A'*A below - const VerticalBlockMatrix& Ab = whitenedFactor->matrixObject(); - - // N is number of variables in HessianFactor, n in JacobianFactor - DenseIndex N = this->info_.nBlocks() - 1, n = Ab.nBlocks() - 1; - - // First build an array of slots - gttic(slots); - FastVector slots(n + 1); - DenseIndex slot = 0; - BOOST_FOREACH(Key j, update) - slots[slot++] = scatter.at(j).slot; - slots[n] = N; - gttoc(slots); - - // Apply updates to the upper triangle - gttic(update); - // Loop over blocks of A, including RHS with j==n - for (DenseIndex j = 0; j <= n; ++j) { - DenseIndex J = slots[j]; // get block in Hessian - // Fill off-diagonal blocks with Ai'*Aj - for (DenseIndex i = 0; i < j; ++i) { - DenseIndex I = slots[i]; // get block in Hessian - info_(I, J).knownOffDiagonal() += Ab(i).transpose() * Ab(j); - } - // Fill diagonal block with Aj'*Aj - info_(J, J).selfadjointView().rankUpdate(Ab(j).transpose()); - } - gttoc(update); - } } /* ************************************************************************* */ diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index dbec5bb59..c3cea3f51 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -363,19 +363,12 @@ namespace gtsam { /** Return the full augmented Hessian matrix of this factor as a SymmetricBlockMatrix object. */ const SymmetricBlockMatrix& matrixObject() const { return info_; } - /** Update the factor by adding the information from the JacobianFactor + /** Update an information matrix by adding the information corresponding to this factor * (used internally during elimination). - * @param update The JacobianFactor containing the new information to add * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param info The information matrix to be updated */ - void updateATA(const JacobianFactor& update, const Scatter& scatter); - - /** Update the factor by adding the information from the HessianFactor - * (used internally during elimination). - * @param update The HessianFactor containing the new information to add - * @param scatter A mapping from variable index to slot index in this HessianFactor - */ - void updateATA(const HessianFactor& update, const Scatter& scatter); + void updateATA(const Scatter& scatter, SymmetricBlockMatrix* info) const; /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 11025fc0f..bb910aedb 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -497,6 +497,49 @@ map JacobianFactor::hessianBlockDiagonal() const { return blocks; } +/* ************************************************************************* */ +void JacobianFactor::updateATA(const Scatter& scatter, + SymmetricBlockMatrix* info) const { + gttic(updateATA_JacobianFactor); + + if (rows() == 0) return; + + // Whiten the factor if it has a noise model + const SharedDiagonal& model = get_model(); + if (model && !model->isUnit()) { + if (model->isConstrained()) + throw invalid_argument( + "JacobianFactor::updateATA: cannot update information with " + "constrained noise model"); + JacobianFactor whitenedFactor = whiten(); + whitenedFactor.updateATA(scatter, info); + } else { + // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below + // N is number of variables in information matrix, n in JacobianFactor + DenseIndex N = info->nBlocks() - 1, n = Ab_.nBlocks() - 1; + + // First build an array of slots + FastVector slots(n + 1); + DenseIndex slot = 0; + BOOST_FOREACH (Key key, *this) + slots[slot++] = scatter.at(key).slot; + slots[n] = N; + + // Apply updates to the upper triangle + // Loop over blocks of A, including RHS with j==n + for (DenseIndex j = 0; j <= n; ++j) { + DenseIndex J = slots[j]; + // Fill off-diagonal blocks with Ai'*Aj + for (DenseIndex i = 0; i < j; ++i) { + DenseIndex I = slots[i]; + (*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j); + } + // Fill diagonal block with Aj'*Aj + (*info)(J, J).selfadjointView().rankUpdate(Ab_(j).transpose()); + } + } +} + /* ************************************************************************* */ Vector JacobianFactor::operator*(const VectorValues& x) const { Vector Ax = zero(Ab_.rows()); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 194ba5c67..73f992770 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -273,6 +273,13 @@ namespace gtsam { /** Get a view of the A matrix */ ABlock getA() { return Ab_.range(0, size()); } + /** Update an information matrix by adding the information corresponding to this factor + * (used internally during elimination). + * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param info The information matrix to be updated + */ + void updateATA(const Scatter& scatter, SymmetricBlockMatrix* info) const; + /** Return A*x */ Vector operator*(const VectorValues& x) const;