diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 3fdad19c6..e2537b7b4 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -20,6 +20,7 @@ #pragma once +#include #include #include @@ -88,6 +89,16 @@ namespace gtsam { /** Return the dimension of the variable pointed to by the given key iterator */ virtual size_t getDim(const_iterator variable) const = 0; + /** Return the augmented information matrix represented by this GaussianFactor. + * The augmented information matrix contains the information matrix with an + * additional column holding the information vector, and an additional row + * holding the transpose of the information vector. The lower-right entry + * contains the constant error term (when \f$ \delta x = 0 \f$). The + * augmented information matrix is described in more detail in HessianFactor, + * which in fact stores an augmented information matrix. + */ + virtual Matrix computeInformation() const = 0; + /** Clone a factor (make a deep copy) */ virtual GaussianFactor::shared_ptr clone() const = 0; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index ebe94514b..8628c95ef 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -188,7 +188,7 @@ HessianFactor::HessianFactor(const std::vector& js, const std::vectorsize(), this->size(), 0); } +/* ************************************************************************* */ +Matrix HessianFactor::computeInformation() const { + return info_.full().selfadjointView(); +} + /* ************************************************************************* */ double HessianFactor::error(const VectorValues& c) const { // error 0.5*(f - 2*x'*g + x'*G*x) @@ -345,7 +350,7 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte // First build an array of slots tic(1, "slots"); - size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. + size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. size_t slot = 0; BOOST_FOREACH(Index j, update) { slots[slot] = scatter.find(j)->second.slot; @@ -399,7 +404,7 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt // First build an array of slots tic(1, "slots"); - size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. + size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. size_t slot = 0; BOOST_FOREACH(Index j, update) { slots[slot] = scatter.find(j)->second.slot; diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index d6ff23159..1454e7a36 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -269,6 +269,23 @@ namespace gtsam { /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ constColumn linearTerm() const; + + /** Return the augmented information matrix represented by this GaussianFactor. + * The augmented information matrix contains the information matrix with an + * additional column holding the information vector, and an additional row + * holding the transpose of the information vector. The lower-right entry + * contains the constant error term (when \f$ \delta x = 0 \f$). The + * augmented information matrix is described in more detail in HessianFactor, + * which in fact stores an augmented information matrix. + * + * For HessianFactor, this is the same as info() except that this function + * returns a complete symmetric matrix whereas info() returns a matrix where + * only the upper triangle is valid, but should be interpreted as symmetric. + * This is because info() returns only a reference to the internal + * representation of the augmented information matrix, which stores only the + * upper triangle. + */ + virtual Matrix computeInformation() const; // Friend unit test classes friend class ::ConversionConstructorHessianFactorTest; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 37fd74c39..2b884e1bd 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -118,7 +118,7 @@ namespace gtsam { GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { - size_t* dims = (size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google. + size_t* dims = (size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google. for(size_t j=0; j >::const_iterator term=terms.begin(); for(; term!=terms.end(); ++term,++j) @@ -271,6 +271,12 @@ namespace gtsam { return 0.5 * weighted.dot(weighted); } + /* ************************************************************************* */ + Matrix JacobianFactor::computeInformation() const { + Matrix AbWhitened = Ab_.full(); + model_->WhitenInPlace(AbWhitened); + return AbWhitened.transpose() * AbWhitened; + } /* ************************************************************************* */ Vector JacobianFactor::operator*(const VectorValues& x) const { diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index b5a2dd500..048d44981 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -154,6 +154,16 @@ namespace gtsam { Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */ Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */ virtual double error(const VectorValues& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */ + + /** Return the augmented information matrix represented by this GaussianFactor. + * The augmented information matrix contains the information matrix with an + * additional column holding the information vector, and an additional row + * holding the transpose of the information vector. The lower-right entry + * contains the constant error term (when \f$ \delta x = 0 \f$). The + * augmented information matrix is described in more detail in HessianFactor, + * which in fact stores an augmented information matrix. + */ + virtual Matrix computeInformation() const; /** Check if the factor contains no information, i.e. zero rows. This does * not necessarily mean that the factor involves no variables (to check for diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 61f7f8dad..936cfb806 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -69,16 +69,9 @@ Matrix Marginals::marginalInformation(Key variable) const { marginalFactor = bayesTree_.marginalFactor(index, EliminateQR); // Get information matrix (only store upper-right triangle) - if(typeid(*marginalFactor) == typeid(JacobianFactor)) { - JacobianFactor::constABlock A = static_cast(*marginalFactor).getA(); - return A.transpose() * A; // Compute A'A - } else if(typeid(*marginalFactor) == typeid(HessianFactor)) { - const HessianFactor& hessian = static_cast(*marginalFactor); - const size_t dim = hessian.getDim(hessian.begin()); - return hessian.info().topLeftCorner(dim,dim).selfadjointView(); // Take the non-augmented part of the information matrix - } else { - throw runtime_error("Internal error: Marginals::marginalInformation expected either a JacobianFactor or HessianFactor"); - } + Matrix info = marginalFactor->computeInformation(); + const int dim = info.rows() - 1; + return info.topLeftCorner(dim,dim); // Take the non-augmented part of the information matrix } /* ************************************************************************* */