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

@@ -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)

gtsam/linear/HessianFactor.h

@@ -270,6 +270,23 @@ namespace gtsam {
      * @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;
     friend class ::Constructor1HessianFactorTest;

gtsam/linear/JacobianFactor.cpp

@@ -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

@@ -155,6 +155,16 @@ namespace gtsam {
     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
      * involving no variables use keys().empty()).

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<const JacobianFactor&>(*marginalFactor).getA();
-    return A.transpose() * A; // Compute A'A
-  } 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");
-  }
+  Matrix info = marginalFactor->computeInformation();
+  const int dim = info.rows() - 1;
+  return info.topLeftCorner(dim,dim); // Take the non-augmented part of the information matrix
 }
 
 /* ************************************************************************* */