Augmented and regular information now implemented

gtsam/slam/RegularImplicitSchurFactor.h

@@ -18,7 +18,7 @@ namespace gtsam {
 /**
  * RegularImplicitSchurFactor
  */
-template<size_t D, size_t Z=2> //
+template<size_t D, size_t Z = 2> //
 class RegularImplicitSchurFactor: public GaussianFactor {
 
 public:
@@ -170,8 +170,12 @@ public:
   static void updateSparseSchurComplement(
       const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
       const Matrix3& P /*Point Covariance*/, const Vector& b, const double f,
-      const FastVector<Key>& keys, const FastMap<Key, size_t>& KeySlotMap,
+      const FastVector<Key>& allKeys, const FastVector<Key>& keys,
       /*output ->*/SymmetricBlockMatrix& augmentedHessian) {
+
+    FastMap<Key, size_t> KeySlotMap;
+    for (size_t slot = 0; slot < allKeys.size(); slot++)
+      KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
     // Schur complement trick
     // G = F' * F - F' * E * P * E' * F
     // g = F' * (b - E * P * E' * b)
@@ -232,15 +236,28 @@ public:
     augmentedHessian(aug_m, aug_m)(0, 0) += f;
   }
 
+  /// *Compute* full augmented information matrix
   virtual Matrix augmentedInformation() const {
-    throw std::runtime_error(
+
-        "RegularImplicitSchurFactor::augmentedInformation non implemented");
+    // Create a SymmetricBlockMatrix
-    return Matrix();
+    int m = this->keys_.size();
+    size_t M1 = D * m + 1;
+    std::vector<DenseIndex> dims(m + 1); // this also includes the b term
+    std::fill(dims.begin(), dims.end() - 1, D);
+    dims.back() = 1;
+    SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
+
+    // Do the Schur complement
+    sparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian);
+    return augmentedHessian.matrix();
   }
+
+  /// *Compute* full information matrix
   virtual Matrix information() const {
-    throw std::runtime_error(
+    Matrix augmented = augmentedInformation();
-        "RegularImplicitSchurFactor::information non implemented");
+    int m = this->keys_.size();
-    return Matrix();
+    size_t M = D * m;
+    return augmented.block(0,0,M,M);
   }
 
   /// Return the diagonal of the Hessian for this factor

gtsam/slam/tests/testRegularImplicitSchurFactor.cpp

@@ -255,6 +255,18 @@ TEST(regularImplicitSchurFactor, hessianDiagonal)
   EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0]));
   EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1]));
   EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
+
+  // augmentedInformation (test just checks diagonals)
+  Matrix actualInfo = factor.augmentedInformation();
+  EXPECT(assert_equal(actualBD[0],actualInfo.block<6,6>(0,0)));
+  EXPECT(assert_equal(actualBD[1],actualInfo.block<6,6>(6,6)));
+  EXPECT(assert_equal(actualBD[3],actualInfo.block<6,6>(12,12)));
+
+  // information (test just checks diagonals)
+  Matrix actualInfo2 = factor.information();
+  EXPECT(assert_equal(actualBD[0],actualInfo2.block<6,6>(0,0)));
+  EXPECT(assert_equal(actualBD[1],actualInfo2.block<6,6>(6,6)));
+  EXPECT(assert_equal(actualBD[3],actualInfo2.block<6,6>(12,12)));
 }
 
 /* ************************************************************************* */