last updates from smartFactors project (these files are now removed from that project)

gtsam_unstable/slam/ImplicitSchurFactor.h

@@ -1,7 +1,8 @@
 /**
  * @file    ImplicitSchurFactor.h
- * @brief   A new type of linear factor (GaussianFactor), which is subclass of JacobiaFactor
+ * @brief   A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor
  * @author  Frank Dellaert
+ * @author  Luca Carlone
  */
 
 #pragma once

gtsam_unstable/slam/JacobianFactorQR.h

@@ -0,0 +1,53 @@
+/*
+ * @file  JacobianFactorQR.h
+ * @brief Jacobianfactor that combines and eliminates points
+ * @date  Oct 27, 2013
+ * @uthor Frank Dellaert
+ */
+
+#pragma once
+#include <gtsam_unstable/slam/JacobianSchurFactor.h>
+
+namespace gtsam {
+/**
+ * JacobianFactor for Schur complement that uses Q noise model
+ */
+template<size_t D>
+class JacobianFactorQR: public JacobianSchurFactor<D> {
+
+  typedef JacobianSchurFactor<D> Base;
+
+public:
+
+  /**
+   * Constructor
+   */
+  JacobianFactorQR(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
+      const Matrix& E, const Matrix3& P, const Vector& b,
+      const SharedDiagonal& model = SharedDiagonal()) :
+      JacobianSchurFactor<D>() {
+    // Create a number of Jacobian factors in a factor graph
+    GaussianFactorGraph gfg;
+    Symbol pointKey('p', 0);
+    size_t i = 0;
+    BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) {
+      gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second,
+          b.segment<2>(2 * i), model);
+      i += 1;
+    }
+    //gfg.print("gfg");
+
+    // eliminate the point
+    GaussianBayesNet::shared_ptr bn;
+    GaussianFactorGraph::shared_ptr fg;
+    std::vector < Key > variables;
+    variables.push_back(pointKey);
+    boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
+    //fg->print("fg");
+
+    JacobianFactor::operator=(JacobianFactor(*fg));
+  }
+};
+// class
+
+}// gtsam

gtsam_unstable/slam/JacobianFactorSVD.h

@@ -0,0 +1,44 @@
+/*
+ * @file  JacobianFactorSVD.h
+ * @date  Oct 27, 2013
+ * @uthor Frank Dellaert
+ */
+
+#pragma once
+#include "gtsam_unstable/slam/JacobianSchurFactor.h"
+
+namespace gtsam {
+/**
+ * JacobianFactor for Schur complement that uses Q noise model
+ */
+template<size_t D>
+class JacobianFactorSVD: public JacobianSchurFactor<D> {
+
+public:
+
+  typedef Eigen::Matrix<double, 2, D> Matrix2D;
+  typedef std::pair<Key, Matrix2D> KeyMatrix2D;
+
+  /// Default constructor
+  JacobianFactorSVD() {}
+
+  /// Constructor
+  JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
+      const SharedDiagonal& model =  SharedDiagonal()) : JacobianSchurFactor<D>() {
+    size_t numKeys = Enull.rows() / 2;
+    size_t j = 0, m2 = 2*numKeys-3;
+    // PLAIN NULL SPACE TRICK
+    // Matrix Q = Enull * Enull.transpose();
+    // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
+    //   QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
+    // JacobianFactor factor(QF, Q * b);
+    typedef std::pair<Key, Matrix> KeyMatrix;
+    std::vector<KeyMatrix> QF;
+    QF.reserve(numKeys);
+    BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
+    QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, 2 * j++, m2, 2) * it.second));
+    JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
+  }
+};
+
+}

gtsam_unstable/slam/SmartFactorBase.h

@@ -51,6 +51,7 @@ protected:
   typedef Eigen::Matrix<double, D, 2> MatrixD2; // F'
   typedef std::pair<Key, Matrix2D> KeyMatrix2D; // Fblocks
   typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block
+  typedef Eigen::Matrix<double, 2, 3> Matrix23;
   typedef Eigen::Matrix<double, D, 1> VectorD;
   typedef Eigen::Matrix<double, 2, 2> Matrix2;
 
@@ -321,7 +322,7 @@ public:
       const double lambda = 0.0) const {
 
     int numKeys = this->keys_.size();
-    std::vector<KeyMatrix2D> Fblocks;
+    std::vector < KeyMatrix2D > Fblocks;
     double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
         lambda);
     F = zeros(2 * numKeys, D * numKeys);
@@ -335,12 +336,13 @@ public:
   // ****************************************************************************************************
   /// SVD version
   double computeJacobiansSVD(std::vector<KeyMatrix2D>& Fblocks, Matrix& Enull,
-      Vector& b, const Cameras& cameras, const Point3& point,
+      Vector& b, const Cameras& cameras, const Point3& point, double lambda =
-      double lambda = 0.0, bool diagonalDamping = false) const {
+          0.0, bool diagonalDamping = false) const {
 
     Matrix E;
     Matrix3 PointCov; // useless
-    double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); // diagonalDamping should have no effect (only on PointCov)
+    double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
+        diagonalDamping); // diagonalDamping should have no effect (only on PointCov)
 
     // Do SVD on A
     Eigen::JacobiSVD < Matrix > svd(E, Eigen::ComputeFullU);
@@ -359,7 +361,7 @@ public:
       const Cameras& cameras, const Point3& point) const {
 
     int numKeys = this->keys_.size();
-    std::vector<KeyMatrix2D> Fblocks;
+    std::vector < KeyMatrix2D > Fblocks;
     double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
     F.resize(2 * numKeys, D * numKeys);
     F.setZero();
@@ -378,13 +380,15 @@ public:
 
     int numKeys = this->keys_.size();
 
-    std::vector<KeyMatrix2D> Fblocks;
+    std::vector < KeyMatrix2D > Fblocks;
     Matrix E;
     Matrix3 PointCov;
     Vector b;
     double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
         diagonalDamping);
 
+//#define HESSIAN_BLOCKS
+#ifdef HESSIAN_BLOCKS
     // Create structures for Hessian Factors
     std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
     std::vector < Vector > gs(numKeys);
@@ -397,8 +401,44 @@ public:
 
     return boost::make_shared < RegularHessianFactor<D>
         > (this->keys_, Gs, gs, f);
+#else
+    size_t n1 = D * numKeys + 1;
+    std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
+    std::fill(dims.begin(), dims.end() - 1, D);
+    dims.back() = 1;
+
+    SymmetricBlockMatrix  augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1)
+    sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
+    augmentedHessian(numKeys,numKeys)(0,0) = f;
+    return boost::make_shared<RegularHessianFactor<D> >(
+            this->keys_, augmentedHessian);
+
+#endif
   }
 
+  // ****************************************************************************************************
+   boost::shared_ptr<RegularHessianFactor<D> > updateAugmentedHessian(
+       const Cameras& cameras, const Point3& point, const double lambda,
+       bool diagonalDamping, SymmetricBlockMatrix& augmentedHessian) const {
+
+     int numKeys = this->keys_.size();
+
+     std::vector < KeyMatrix2D > Fblocks;
+     Matrix E;
+     Matrix3 PointCov;
+     Vector b;
+     double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
+         diagonalDamping);
+
+     std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
+     std::fill(dims.begin(), dims.end() - 1, D);
+     dims.back() = 1;
+
+     updateSparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
+     std::cout << "f "<< f <<std::endl;
+     augmentedHessian(numKeys,numKeys)(0,0) += f;
+   }
+
   // ****************************************************************************************************
   void schurComplement(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
       const Matrix& PointCov, const Vector& b,
@@ -437,47 +477,133 @@ public:
   }
 
   // ****************************************************************************************************
-  void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
+  void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
-      const Matrix& E, const Matrix& PointCov, const Vector& b,
+      const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
-      /*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
+      /*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
     // Schur complement trick
-    // Gs = F' * F - F' * E * inv(E'*E) * E' * F
+    // Gs = F' * F - F' * E * P * E' * F
-    // gs = F' * (b - E * inv(E'*E) * E' * b)
+    // gs = F' * (b - E * P * E' * b)
+
+    // a single point is observed in numKeys cameras
+    size_t numKeys = this->keys_.size(); // cameras observing current point
+    size_t aug_numKeys = augmentedHessian.rows() - 1; // all cameras in the group
+
+    // Blockwise Schur complement
+    for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
+
+      const Matrix2D& Fi1 = Fblocks.at(i1).second;
+      const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P;
+
+      // D = (Dx2) * (2)
+      // (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
+      size_t aug_i1 = this->keys_[i1];
+      std::cout << "i1 "<< i1 <<std::endl;
+      std::cout << "aug_i1 "<< aug_i1 <<std::endl;
+      std::cout << "aug_numKeys "<< aug_numKeys <<std::endl;
+      augmentedHessian(aug_i1,aug_numKeys) = //augmentedHessian(aug_i1,aug_numKeys) +
+                        Fi1.transpose() * b.segment < 2 > (2 * i1) // F' * b
+                      - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
+
+      // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
+      std::cout << "filled 1  " <<std::endl;
+      augmentedHessian(aug_i1,aug_i1) = //augmentedHessian(aug_i1,aug_i1) +
+          Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
+
+      // upper triangular part of the hessian
+      for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera
+        const Matrix2D& Fi2 = Fblocks.at(i2).second;
+        size_t aug_i2 = this->keys_[i2];
+        std::cout << "i2 "<< i2 <<std::endl;
+        std::cout << "aug_i2 "<< aug_i2 <<std::endl;
+
+        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
+        augmentedHessian(aug_i1, aug_i2) = //augmentedHessian(aug_i1, aug_i2)
+            - Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
+      }
+    } // end of for over cameras
+  }
+
+  // ****************************************************************************************************
+  void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
+      const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
+      /*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
+    // Schur complement trick
+    // Gs = F' * F - F' * E * P * E' * F
+    // gs = F' * (b - E * P * E' * b)
 
     // a single point is observed in numKeys cameras
     size_t numKeys = this->keys_.size();
 
     // Blockwise Schur complement
-    int GsCount = 0;
     for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
-      const Matrix2D& Fi1 = Fblocks.at(i1).second;
-      // D = (Dx2) * (2)
-      gs.at(i1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
 
-      for (size_t i2 = 0; i2 < numKeys; i2++) {
+      const Matrix2D& Fi1 = Fblocks.at(i1).second;
+      const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P;
+
+      // D = (Dx2) * (2)
+      // (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
+      augmentedHessian(i1,numKeys) = Fi1.transpose() * b.segment < 2 > (2 * i1) // F' * b
+                      - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
+
+      // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
+      augmentedHessian(i1,i1) =
+          Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
+
+      // upper triangular part of the hessian
+      for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera
         const Matrix2D& Fi2 = Fblocks.at(i2).second;
 
-        // Compute (Ei1 * PointCov * Ei2')
+        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
-        // (2x2) = (2x3) * (3x3) * (3x2)
+        augmentedHessian(i1,i2) =
-        Matrix2 E_invEtE_Et = E.block<2, 3>(2 * i1, 0) * PointCov * (E.block<2, 3>(2 * i2, 0)).transpose();
+            -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
-
-        // D = (Dx2) * (2x2) * (2)
-        gs.at(i1) -= Fi1.transpose() *  ( E_invEtE_Et * b.segment < 2 > (2 * i2) );
-
-        if (i2 == i1) { // diagonal entries
-          // (DxD) = (Dx2) * ( (2xD) - (2x2) * (2xD) )
-          Gs.at(GsCount) = Fi1.transpose() * (Fi1 -  E_invEtE_Et * Fi2);
-          GsCount++;
-        }
-        if (i2 > i1) { // off diagonal
-          // (DxD) = (Dx2) * ( (2x2) * (2xD) )
-          Gs.at(GsCount) = - Fi1.transpose() *  ( E_invEtE_Et * Fi2 );
-          GsCount++;
-        }
       }
-    }
+    } // end of for over cameras
   }
 
+  // ****************************************************************************************************
+  void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
+      const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
+      /*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
+    // Schur complement trick
+    // Gs = F' * F - F' * E * P * E' * F
+    // gs = F' * (b - E * P * E' * b)
+
+    // a single point is observed in numKeys cameras
+    size_t numKeys = this->keys_.size();
+
+    int GsIndex = 0;
+    // Blockwise Schur complement
+    for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
+      // GsIndex points to the upper triangular blocks
+      // 0  1  2  3  4
+      // X  5  6  7  8
+      // X  X  9 10 11
+      // X  X  X 12 13
+      // X  X  X X  14
+      const Matrix2D& Fi1 = Fblocks.at(i1).second;
+
+      const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P;
+
+      { // for i1 = i2
+        // D = (Dx2) * (2)
+        gs.at(i1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
+        // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
+        gs.at(i1) -= Fi1.transpose() * (Ei1_P * (E.transpose() * b));
+
+        // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
+        Gs.at(GsIndex) = Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
+        GsIndex++;
+      }
+      // upper triangular part of the hessian
+      for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera
+        const Matrix2D& Fi2 = Fblocks.at(i2).second;
+
+        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
+        Gs.at(GsIndex) = -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
+        GsIndex++;
+      }
+    } // end of for over cameras
+  }
   // ****************************************************************************************************
   boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
       const Cameras& cameras, const Point3& point, double lambda = 0.0,
@@ -494,7 +620,7 @@ public:
   boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
       const Cameras& cameras, const Point3& point, double lambda = 0.0,
       bool diagonalDamping = false) const {
-    std::vector<KeyMatrix2D> Fblocks;
+    std::vector < KeyMatrix2D > Fblocks;
     Matrix E;
     Matrix3 PointCov;
     Vector b;