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

@@ -20,6 +20,7 @@
 
 #pragma once
 
+#include <gtsam/base/Matrix.h>
 #include <gtsam/inference/IndexFactor.h>
 
 #include <string>
@@ -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;
 

gtsam/linear/HessianFactor.cpp

@@ -188,7 +188,7 @@ HessianFactor::HessianFactor(const std::vector<Index>& js, const std::vector<Mat
   }
 
   // Create the dims vector
-  size_t* dims = (size_t*)alloca(sizeof(size_t)*(variable_count+1)); // FIXME: alloca is bad, just ask Google.
+  size_t* dims = (size_t*)alloca(sizeof(size_t)*(variable_count+1)); // FIXME: alloca is bad, just ask Google.
   size_t total_size = 0;
   for(unsigned int i = 0; i < variable_count; ++i){
     dims[i] = gs[i].size();
@@ -324,6 +324,11 @@ HessianFactor::constColumn HessianFactor::linearTerm() const {
 	return info_.rangeColumn(0, this->size(), this->size(), 0);
 }
 
+/* ************************************************************************* */
+Matrix HessianFactor::computeInformation() const {
+  return info_.full().selfadjointView<Eigen::Upper>();
+}
+
 /* ************************************************************************* */
 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;

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;

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<terms.size(); ++j)
       dims[j] = terms[j].second.cols();
     dims[terms.size()] = 1;
@@ -135,7 +135,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.
     size_t j=0;
     std::list<std::pair<Index, Matrix> >::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 {

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

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)) {
+  Matrix info = marginalFactor->computeInformation();
-    JacobianFactor::constABlock A = static_cast<const JacobianFactor&>(*marginalFactor).getA();
+  const int dim = info.rows() - 1;
-    return A.transpose() * A; // Compute A'A
+  return info.topLeftCorner(dim,dim); // Take the non-augmented part of the information matrix
-  } else if(typeid(*marginalFactor) == typeid(HessianFactor)) {
-    const HessianFactor& hessian = static_cast<const HessianFactor&>(*marginalFactor);
-    const size_t dim = hessian.getDim(hessian.begin());
-    return hessian.info().topLeftCorner(dim,dim).selfadjointView<Eigen::Upper>(); // Take the non-augmented part of the information matrix
-  } else {
-    throw runtime_error("Internal error: Marginals::marginalInformation expected either a JacobianFactor or HessianFactor");
-  }
 }
 
 /* ************************************************************************* */