Cleaned up information/augmentedInformation for GaussianFactor and GaussianConditional

gtsam.h

@@ -1078,7 +1078,8 @@ class GaussianConditional {
   size_t dim() const;
 
   //Advanced Interface
-  Matrix computeInformation() const;
+  Matrix information() const;
+  Matrix augmentedInformation() const;
   gtsam::JacobianFactor* toFactor() const;
   void solveInPlace(gtsam::VectorValues& x) const;
   void solveTransposeInPlace(gtsam::VectorValues& gy) const;
@@ -1136,6 +1137,8 @@ virtual class GaussianFactor {
   bool equals(const gtsam::GaussianFactor& lf, double tol) const;
   double error(const gtsam::VectorValues& c) const;
   gtsam::GaussianFactor* negate() const;
+  Matrix augmentedInformation() const;
+  Matrix information() const;
   size_t size() const;
 };
 
@@ -1161,7 +1164,6 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
   double error(const gtsam::VectorValues& c) const;
 
   //Standard Interface
-    gtsam::GaussianFactor* negate() const;
   bool empty() const;
   Matrix getA() const;
   Vector getb() const;
@@ -1175,7 +1177,6 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
   gtsam::GaussianConditional* eliminateFirst();
   gtsam::GaussianConditional* eliminate(size_t nrFrontals);
   gtsam::GaussianFactor* negate() const;
-    Matrix computeInformation() const;
   void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const;
   gtsam::JacobianFactor whiten() const;
   gtsam::GaussianConditional* eliminateFirst();
@@ -1211,11 +1212,9 @@ virtual class HessianFactor : gtsam::GaussianFactor {
 
   //Standard Interface
   size_t rows() const;
-  gtsam::GaussianFactor* negate() const;
   Matrix info() const;
   double constantTerm() const;
   Vector linearTerm() const;
-  Matrix computeInformation() const;
 
   //Advanced Interface
   void partialCholesky(size_t nrFrontals);

gtsam/linear/GaussianConditional.h

@@ -31,7 +31,7 @@ class eliminate2JacobianFactorTest;
 class constructorGaussianConditionalTest;
 class eliminationGaussianFactorGraphTest;
 class complicatedMarginalGaussianJunctionTreeTest;
-class computeInformationGaussianConditionalTest;
+class informationGaussianConditionalTest;
 class isGaussianFactorGaussianConditionalTest;
 
 namespace gtsam {
@@ -147,8 +147,15 @@ public:
   /** dimension of multivariate variable */
   size_t dim() const { return rsd_.rows(); }
 
+  /** Compute the augmented information matrix as
+   * \f$ [ R S d ]^T [ R S d ] \f$
+   */
+  Matrix augmentedInformation() const {
+    return rsd_.full().transpose() * rsd_.full().transpose();
+  }
+
   /** Compute the information matrix */
-  Matrix computeInformation() const {
+  Matrix information() const {
     return get_R().transpose() * get_R();
   }
 
@@ -217,7 +224,7 @@ private:
   friend class ::constructorGaussianConditionalTest;
   friend class ::eliminationGaussianFactorGraphTest;
   friend class ::complicatedMarginalGaussianJunctionTreeTest;
-  friend class ::computeInformationGaussianConditionalTest;
+  friend class ::informationGaussianConditionalTest;
   friend class ::isGaussianFactorGaussianConditionalTest;
 
   /** Serialization function */

gtsam/linear/GaussianDensity.cpp

@@ -45,11 +45,6 @@ namespace gtsam {
     return x[k];
   }
 
-  /* ************************************************************************* */
-  Matrix GaussianDensity::information() const {
-    return computeInformation();
-  }
-
   /* ************************************************************************* */
   Matrix GaussianDensity::covariance() const {
     return inverse(information());

gtsam/linear/GaussianDensity.h

@@ -60,9 +60,6 @@ namespace gtsam {
     /// Mean \f$ \mu = R^{-1} d \f$
     Vector mean() const;
 
-    /// Information matrix \f$ \Lambda = R^T R \f$
-    Matrix information() const;
-
     /// Covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$
     Matrix covariance() const;
 

gtsam/linear/GaussianFactor.h

@@ -101,7 +101,12 @@ namespace gtsam {
      * augmented information matrix is described in more detail in HessianFactor,
      * which in fact stores an augmented information matrix.
      */
-    virtual Matrix computeInformation() const = 0;
+    virtual Matrix augmentedInformation() const = 0;
+
+    /** Return the non-augmented information matrix represented by this
+     * GaussianFactor.
+     */
+    virtual Matrix information() const = 0;
 
     /** Clone a factor (make a deep copy) */
     virtual GaussianFactor::shared_ptr clone() const = 0;

gtsam/linear/GaussianISAM.cpp

@@ -42,7 +42,7 @@ BayesNet<GaussianConditional>::shared_ptr GaussianISAM::marginalBayesNet(Index j
 /* ************************************************************************* */
 Matrix GaussianISAM::marginalCovariance(Index j) const {
   GaussianConditional::shared_ptr conditional = marginalBayesNet(j)->front();
-  return conditional->computeInformation().inverse();
+  return conditional->information().inverse();
 }
 
 /* ************************************************************************* */

gtsam/linear/GaussianMultifrontalSolver.cpp

@@ -88,7 +88,7 @@ Matrix GaussianMultifrontalSolver::marginalCovariance(Index j) const {
     fg.push_back(Base::marginalFactor(j,&EliminatePreferCholesky));
     conditional = EliminatePreferCholesky(fg,1).first;
   }
-  return conditional->computeInformation().inverse();
+  return conditional->information().inverse();
 }
 
 }

gtsam/linear/GaussianSequentialSolver.cpp

@@ -104,7 +104,7 @@ Matrix GaussianSequentialSolver::marginalCovariance(Index j) const {
     fg.push_back(Base::marginalFactor(j, &EliminatePreferCholesky));
     conditional = EliminatePreferCholesky(fg, 1).first;
   }
-  return conditional->computeInformation().inverse();
+  return conditional->information().inverse();
 }
 
 /* ************************************************************************* */

gtsam/linear/HessianFactor.cpp

@@ -311,10 +311,15 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
 }
 
 /* ************************************************************************* */
-Matrix HessianFactor::computeInformation() const {
+Matrix HessianFactor::augmentedInformation() const {
   return info_.full().selfadjointView<Eigen::Upper>();
 }
 
+/* ************************************************************************* */
+Matrix HessianFactor::information() const {
+  return info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>();
+}
+
 /* ************************************************************************* */
 double HessianFactor::error(const VectorValues& c) const {
   // error 0.5*(f - 2*x'*g + x'*G*x)

gtsam/linear/HessianFactor.h

@@ -311,7 +311,12 @@ namespace gtsam {
      * representation of the augmented information matrix, which stores only the
      * upper triangle.
      */
-    virtual Matrix computeInformation() const;
+    virtual Matrix augmentedInformation() const;
+
+    /** Return the non-augmented information matrix represented by this
+     * GaussianFactor.
+     */
+    virtual Matrix information() const;
 
     // Friend unit test classes
     friend class ::ConversionConstructorHessianFactorTest;

gtsam/linear/JacobianFactor.cpp

@@ -281,12 +281,19 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Matrix JacobianFactor::computeInformation() const {
+  Matrix JacobianFactor::augmentedInformation() const {
     Matrix AbWhitened = Ab_.full();
     model_->WhitenInPlace(AbWhitened);
     return AbWhitened.transpose() * AbWhitened;
   }
 
+  /* ************************************************************************* */
+  Matrix JacobianFactor::information() const {
+    Matrix AWhitened = this->getA();
+    model_->WhitenInPlace(AWhitened);
+    return AWhitened.transpose() * AWhitened;
+  }
+
   /* ************************************************************************* */
   Vector JacobianFactor::operator*(const VectorValues& x) const {
     Vector Ax = zero(Ab_.rows());

gtsam/linear/JacobianFactor.h

@@ -162,7 +162,12 @@ namespace gtsam {
      * augmented information matrix is described in more detail in HessianFactor,
      * which in fact stores an augmented information matrix.
      */
-    virtual Matrix computeInformation() const;
+    virtual Matrix augmentedInformation() const;
+    
+    /** Return the non-augmented information matrix represented by this
+     * GaussianFactor.
+     */
+    virtual Matrix information() const;
 
     /**
      * Construct the corresponding anti-factor to negate information

gtsam/linear/tests/testGaussianConditional.cpp

@@ -306,7 +306,7 @@ TEST( GaussianConditional, solveTranspose ) {
 }
 
 /* ************************************************************************* */
-TEST( GaussianConditional, computeInformation ) {
+TEST( GaussianConditional, information ) {
 
   // Create R matrix
   Matrix R = (Matrix(4,4) <<
@@ -322,7 +322,7 @@ TEST( GaussianConditional, computeInformation ) {
   Matrix IExpected = R.transpose() * R;
 
   // Actual information matrix (conditional should permute R)
-  Matrix IActual = conditional.computeInformation();
+  Matrix IActual = conditional.information();
   EXPECT(assert_equal(IExpected, IActual));
 }
 
@@ -340,7 +340,7 @@ TEST( GaussianConditional, isGaussianFactor ) {
   GaussianConditional conditional(0, Vector::Zero(4), R, Vector::Constant(4, 1.0));
 
   // Expected information matrix computed by conditional
-  Matrix IExpected = conditional.computeInformation();
+  Matrix IExpected = conditional.information();
 
   // Expected information matrix computed by a factor
   JacobianFactor jf = *conditional.toFactor();

gtsam/nonlinear/Marginals.cpp

@@ -71,9 +71,7 @@ Matrix Marginals::marginalInformation(Key variable) const {
     marginalFactor = bayesTree_.marginalFactor(index, EliminateQR);
 
   // Get information matrix (only store upper-right triangle)
-  Matrix info = marginalFactor->computeInformation();
+  return marginalFactor->information();
-  const int dim = info.rows() - 1;
-  return info.topLeftCorner(dim,dim); // Take the non-augmented part of the information matrix
 }
 
 /* ************************************************************************* */