From 6e2312294cf22e86d9237dba756256e5577e45f7 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 23 May 2012 20:56:22 +0000 Subject: [PATCH] Added computeInformation function to GaussianFactor to properly compute information matrix including noise models, and using it to fix bug in Marginals where noise model was not being accounted for (only affects when hard constraints are used) --- gtsam/linear/GaussianFactor.h | 11 +++++++++++ gtsam/linear/HessianFactor.cpp | 11 ++++++++--- gtsam/linear/HessianFactor.h | 17 +++++++++++++++++ gtsam/linear/JacobianFactor.cpp | 10 ++++++++-- gtsam/linear/JacobianFactor.h | 10 ++++++++++ gtsam/nonlinear/Marginals.cpp | 13 +++---------- 6 files changed, 57 insertions(+), 15 deletions(-) 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 } /* ************************************************************************* */