Merged in feature/LM/BlockDiagonal (pull request #8)

hessianBlockDiagonal

gtsam/linear/GaussianFactor.h

@@ -99,6 +99,9 @@ namespace gtsam {
     /// Return the diagonal of the Hessian for this factor
     virtual VectorValues hessianDiagonal() const = 0;
 
+    /// Return the block diagonal of the Hessian for this factor
+    virtual std::map<Key,Matrix> hessianBlockDiagonal() const = 0;
+
     /** Clone a factor (make a deep copy) */
     virtual GaussianFactor::shared_ptr clone() const = 0;
 

gtsam/linear/GaussianFactorGraph.cpp

@@ -79,7 +79,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  std::vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
+  vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
     // First find dimensions of each variable
     vector<size_t> dims;
     BOOST_FOREACH(const sharedFactor& factor, *this) {
@@ -146,7 +146,7 @@ namespace gtsam {
 
     // call sparseJacobian
     typedef boost::tuple<size_t, size_t, double> triplet;
-    std::vector<triplet> result = sparseJacobian();
+    vector<triplet> result = sparseJacobian();
 
     // translate to base 1 matrix
     size_t nzmax = result.size();
@@ -169,7 +169,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  std::pair<Matrix, Vector> GaussianFactorGraph::jacobian(
+  pair<Matrix, Vector> GaussianFactorGraph::jacobian(
       boost::optional<const Ordering&> optionalOrdering) const {
     Matrix augmented = augmentedJacobian(optionalOrdering);
     return make_pair(augmented.leftCols(augmented.cols() - 1),
@@ -188,7 +188,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  std::pair<Matrix, Vector> GaussianFactorGraph::hessian(
+  pair<Matrix, Vector> GaussianFactorGraph::hessian(
       boost::optional<const Ordering&> optionalOrdering) const {
     Matrix augmented = augmentedHessian(optionalOrdering);
     return make_pair(
@@ -206,6 +206,24 @@ namespace gtsam {
     return d;
   }
 
+  /* ************************************************************************* */
+  map<Key,Matrix> GaussianFactorGraph::hessianBlockDiagonal() const {
+    map<Key,Matrix> blocks;
+    BOOST_FOREACH(const sharedFactor& factor, *this) {
+      map<Key,Matrix> BD = factor->hessianBlockDiagonal();
+      map<Key,Matrix>::const_iterator it = BD.begin();
+      for(;it!=BD.end();it++) {
+        Key j = it->first; // variable key for this block
+        const Matrix& Bj = it->second;
+        if (blocks.count(j))
+          blocks[j] += Bj;
+        else
+          blocks.insert(make_pair(j,Bj));
+      }
+    }
+    return blocks;
+  }
+
   /* ************************************************************************* */
   VectorValues GaussianFactorGraph::optimize(OptionalOrdering ordering, const Eliminate& function) const
   {
@@ -217,9 +235,9 @@ namespace gtsam {
   namespace {
     JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) {
       JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast<JacobianFactor>(gf);
-      if( !result ) {
+      if( !result )
-        result = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
+        // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
-      }
+        result = boost::make_shared<JacobianFactor>(*gf);
       return result;
     }
   }

gtsam/linear/GaussianFactorGraph.h

@@ -224,6 +224,9 @@ namespace gtsam {
     /** Return only the diagonal of the Hessian A'*A, as a VectorValues */
     VectorValues hessianDiagonal() const;
 
+    /** Return the block diagonal of the Hessian for this factor */
+    std::map<Key,Matrix> hessianBlockDiagonal() const;
+
     /** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using
      *  the dense elimination function specified in \c function (default EliminatePreferCholesky),
      *  followed by back-substitution in the Bayes tree resulting from elimination.  Is equivalent

gtsam/linear/HessianFactor.cpp

@@ -363,6 +363,18 @@ VectorValues HessianFactor::hessianDiagonal() const {
   return d;
 }
 
+/* ************************************************************************* */
+map<Key,Matrix> HessianFactor::hessianBlockDiagonal() const {
+  map<Key,Matrix> blocks;
+  // Loop over all variables
+  for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
+    // Get the diagonal block, and insert it
+    Matrix B = info_(j, j).selfadjointView();
+    blocks.insert(make_pair(keys_[j],B));
+  }
+  return blocks;
+}
+
 /* ************************************************************************* */
 Matrix HessianFactor::augmentedJacobian() const
 {

gtsam/linear/HessianFactor.h

@@ -339,6 +339,9 @@ namespace gtsam {
     /// Return the diagonal of the Hessian for this factor
     virtual VectorValues hessianDiagonal() const;
 
+    /// Return the block diagonal of the Hessian for this factor
+    virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
+
     /**
      * Return (dense) matrix associated with factor
      * @param ordering of variables needed for matrix column order

gtsam/linear/JacobianFactor.cpp

@@ -455,6 +455,18 @@ namespace gtsam {
     return d;
   }
 
+  /* ************************************************************************* */
+  map<Key,Matrix> JacobianFactor::hessianBlockDiagonal() const {
+    map<Key,Matrix> blocks;
+    for(size_t pos=0; pos<size(); ++pos)
+    {
+      Key j = keys_[pos];
+      Matrix Aj = model_->Whiten(Ab_(pos));
+      blocks.insert(make_pair(j,Aj.transpose()*Aj));
+    }
+    return blocks;
+  }
+
   /* ************************************************************************* */
   Vector JacobianFactor::operator*(const VectorValues& x) const {
     Vector Ax = zero(Ab_.rows());

gtsam/linear/JacobianFactor.h

@@ -184,6 +184,9 @@ namespace gtsam {
     /// Return the diagonal of the Hessian for this factor
     virtual VectorValues hessianDiagonal() const;
 
+    /// Return the block diagonal of the Hessian for this factor
+    virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
+
     /**
      * @brief Returns (dense) A,b pair associated with factor, bakes in the weights
      */

gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp

@@ -112,10 +112,14 @@ TEST(GaussianFactorGraph, matrices) {
   //  9 10  0 11 12 13
   //  0  0  0 14 15 16
 
+  Matrix A00 = (Matrix(2, 3) << 1, 2, 3, 5, 6, 7);
+  Matrix A10 = (Matrix(2, 3) << 9, 10, 0, 0, 0, 0);
+  Matrix A11 = (Matrix(2, 2) << 11, 12, 14, 15);
+
   GaussianFactorGraph gfg;
   SharedDiagonal model = noiseModel::Unit::Create(2);
-  gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.), (Vector(2) << 4., 8.), model);
+  gfg.add(0, A00, (Vector(2) << 4., 8.), model);
-  gfg.add(0, (Matrix(2, 3) << 9.,10., 0., 0., 0., 0.), 1, (Matrix(2, 2) << 11., 12., 14., 15.), (Vector(2) << 13.,16.), model);
+  gfg.add(0, A10, 1, A11, (Vector(2) << 13.,16.), model);
 
   Matrix Ab(4,6);
   Ab <<
@@ -124,25 +128,35 @@ TEST(GaussianFactorGraph, matrices) {
       9,10, 0,11,12,13,
       0, 0, 0,14,15,16;
 
+  // augmented versions
   EXPECT(assert_equal(Ab, gfg.augmentedJacobian()));
   EXPECT(assert_equal(Ab.transpose() * Ab, gfg.augmentedHessian()));
 
+  // jacobian
   Matrix A = Ab.leftCols(Ab.cols()-1);
   Vector b = Ab.col(Ab.cols()-1);
   Matrix actualA; Vector actualb; boost::tie(actualA,actualb) = gfg.jacobian();
   EXPECT(assert_equal(A, actualA));
   EXPECT(assert_equal(b, actualb));
 
+  // hessian
   Matrix L = A.transpose() * A;
   Vector eta = A.transpose() * b;
   Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian();
   EXPECT(assert_equal(L, actualL));
   EXPECT(assert_equal(eta, actualeta));
 
+  // hessianBlockDiagonal
   VectorValues expectLdiagonal; // Make explicit that diagonal is sum-squares of columns
   expectLdiagonal.insert(0, (Vector(3) << 1+25+81, 4+36+100, 9+49));
   expectLdiagonal.insert(1, (Vector(2) << 121+196, 144+225));
   EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal()));
+
+  // hessianBlockDiagonal
+  map<Key,Matrix> actualBD = gfg.hessianBlockDiagonal();
+  LONGS_EQUAL(2,actualBD.size());
+  EXPECT(assert_equal(A00.transpose()*A00 + A10.transpose()*A10,actualBD[0]));
+  EXPECT(assert_equal(A11.transpose()*A11,actualBD[1]));
 }
 
 /* ************************************************************************* */

gtsam/linear/tests/testHessianFactorUnordered.cpp

@@ -460,11 +460,18 @@ TEST(HessianFactor, hessianDiagonal)
 
   HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f);
 
+  // hessianDiagonal
   VectorValues expected;
   expected.insert(0, (Vector(1) << 1));
   expected.insert(1, (Vector(2) << 1,1));
   EXPECT(assert_equal(expected, factor.hessianDiagonal()));
-}
+
+  // hessianBlockDiagonal
+  map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
+  LONGS_EQUAL(2,actualBD.size());
+  EXPECT(assert_equal(G11,actualBD[0]));
+  EXPECT(assert_equal(G22,actualBD[1]));
+  }
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}

gtsam/linear/tests/testJacobianFactorUnordered.cpp

@@ -240,6 +240,13 @@ TEST(JacobianFactor, matrices)
   expectDiagonal.insert(10, (Vector(3) << 4, 4, 4)/0.25);
   expectDiagonal.insert(15, (Vector(3) << 9, 9, 9)/0.25);
   EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal()));
+
+  // hessianBlockDiagonal
+  map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
+  LONGS_EQUAL(3,actualBD.size());
+  EXPECT(assert_equal(4*eye(3),actualBD[5]));
+  EXPECT(assert_equal(16*eye(3),actualBD[10]));
+  EXPECT(assert_equal(36*eye(3),actualBD[15]));
 }
 
 /* ************************************************************************* */