Moved SchurComplement here, as well as UpdateSchurComplement

gtsam/geometry/CameraSet.h

@@ -21,6 +21,8 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/SymmetricBlockMatrix.h>
+#include <gtsam/base/FastMap.h>
 #include <vector>
 
 namespace gtsam {
@@ -39,8 +41,8 @@ protected:
    */
   typedef typename CAMERA::Measurement Z;
 
+  static const int D = traits<CAMERA>::dimension; ///< Camera dimension
   static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
-  static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
 
   /// Make a vector of re-projection errors
   static Vector ErrorVector(const std::vector<Z>& predicted,
@@ -63,7 +65,7 @@ protected:
 public:
 
   /// Definitions for blocks of F
-  typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
+  typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
   typedef std::vector<MatrixZD> FBlocks;
 
   /**
@@ -97,7 +99,7 @@ public:
    * throws CheiralityException
    */
   std::vector<Z> project2(const Point3& point, //
-      boost::optional<FBlocks&> F = boost::none, //
+      boost::optional<FBlocks&> Fs = boost::none, //
       boost::optional<Matrix&> E = boost::none) const {
 
     // Allocate result
@@ -110,11 +112,11 @@ public:
 
     // Project and fill derivatives
     for (size_t i = 0; i < m; i++) {
-      Eigen::Matrix<double, ZDim, Dim> Fi;
+      MatrixZD Fi;
       Eigen::Matrix<double, ZDim, 3> Ei;
-      z[i] = this->at(i).project2(point, F ? &Fi : 0, E ? &Ei : 0);
+      z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0);
-      if (F)
+      if (Fs)
-        F->push_back(Fi);
+        Fs->push_back(Fi);
       if (E)
         E->block<ZDim, 3>(ZDim * i, 0) = Ei;
     }
@@ -141,9 +143,9 @@ public:
 
   /// Calculate vector of re-projection errors
   Vector reprojectionError(const Point3& point, const std::vector<Z>& measured,
-      boost::optional<FBlocks&> F = boost::none, //
+      boost::optional<FBlocks&> Fs = boost::none, //
       boost::optional<Matrix&> E = boost::none) const {
-    return ErrorVector(project2(point, F, E), measured);
+    return ErrorVector(project2(point, Fs, E), measured);
   }
 
   /// Calculate vector of re-projection errors, from point at infinity
@@ -153,6 +155,127 @@ public:
     return ErrorVector(projectAtInfinity(point), measured);
   }
 
+  /**
+   * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
+   * G = F' * F - F' * E * P * E' * F
+   * g = F' * (b - E * P * E' * b)
+   */
+  static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs,
+      const Matrix& E, const Matrix3& P, const Vector& b) {
+
+    // a single point is observed in m cameras
+    int m = Fs.size();
+
+    // Create a SymmetricBlockMatrix
+    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));
+
+    // Blockwise Schur complement
+    for (size_t i = 0; i < m; i++) { // for each camera
+
+      const MatrixZD& Fi = Fs[i];
+      const Matrix23 Ei_P = E.block<ZDim, 3>(ZDim * i, 0) * P;
+
+      // D = (Dx2) * ZDim
+      augmentedHessian(i, m) = Fi.transpose() * b.segment<ZDim>(ZDim * i) // F' * b
+      - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
+
+      // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
+      augmentedHessian(i, i) = Fi.transpose()
+          * (Fi - Ei_P * E.block<ZDim, 3>(ZDim * i, 0).transpose() * Fi);
+
+      // upper triangular part of the hessian
+      for (size_t j = i + 1; j < m; j++) { // for each camera
+        const MatrixZD& Fj = Fs[j];
+
+        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
+        augmentedHessian(i, j) = -Fi.transpose()
+            * (Ei_P * E.block<ZDim, 3>(ZDim * j, 0).transpose() * Fj);
+      }
+    } // end of for over cameras
+
+    augmentedHessian(m, m)(0, 0) += b.squaredNorm();
+    return augmentedHessian;
+  }
+
+  /**
+   * Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
+   * and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
+   */
+  static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E,
+      const Matrix3& P /*Point Covariance*/, const Vector& b,
+      const FastVector<Key>& allKeys, const FastVector<Key>& keys,
+      /*output ->*/SymmetricBlockMatrix& augmentedHessian) {
+
+    assert(keys.size()==Fs.size());
+    assert(keys.size()<=allKeys.size());
+    
+    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)
+
+    Eigen::Matrix<double, D, D> matrixBlock;
+    typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
+
+    // a single point is observed in m cameras
+    size_t m = Fs.size(); // cameras observing current point
+    size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group
+    assert(allKeys.size()==M);
+
+    // Blockwise Schur complement
+    for (size_t i = 0; i < m; i++) { // for each camera in the current factor
+
+      const MatrixZD& Fi = Fs[i];
+      const Matrix23 Ei_P = E.block<ZDim, 3>(ZDim * i, 0) * P;
+
+      // D = (DxZDim) * (ZDim)
+      // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
+      // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4)
+      // Key cameraKey_i = this->keys_[i];
+      DenseIndex aug_i = KeySlotMap.at(keys[i]);
+
+      // information vector - store previous vector
+      // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal();
+      // add contribution of current factor
+      augmentedHessian(aug_i, M) = augmentedHessian(aug_i, M).knownOffDiagonal()
+          + Fi.transpose() * b.segment<ZDim>(ZDim * i) // F' * b
+      - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
+
+      // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
+      // main block diagonal - store previous block
+      matrixBlock = augmentedHessian(aug_i, aug_i);
+      // add contribution of current factor
+      augmentedHessian(aug_i, aug_i) = matrixBlock
+          + (Fi.transpose()
+              * (Fi - Ei_P * E.block<ZDim, 3>(ZDim * i, 0).transpose() * Fi));
+
+      // upper triangular part of the hessian
+      for (size_t j = i + 1; j < m; j++) { // for each camera
+        const MatrixZD& Fj = Fs[j];
+
+        DenseIndex aug_j = KeySlotMap.at(keys[j]);
+
+        // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
+        // off diagonal block - store previous block
+        // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal();
+        // add contribution of current factor
+        augmentedHessian(aug_i, aug_j) =
+            augmentedHessian(aug_i, aug_j).knownOffDiagonal()
+                - Fi.transpose()
+                    * (Ei_P * E.block<ZDim, 3>(ZDim * j, 0).transpose() * Fj);
+      }
+    } // end of for over cameras
+
+    augmentedHessian(M, M)(0, 0) += b.squaredNorm();
+  }
+
 private:
 
   /// Serialization function
@@ -163,6 +286,9 @@ private:
   }
 };
 
+template<class CAMERA>
+const int CameraSet<CAMERA>::D;
+
 template<class CAMERA>
 const int CameraSet<CAMERA>::ZDim;
 

gtsam/geometry/tests/testCameraSet.cpp

@@ -31,14 +31,15 @@ using namespace gtsam;
 #include <gtsam/geometry/Cal3Bundler.h>
 TEST(CameraSet, Pinhole) {
   typedef PinholeCamera<Cal3Bundler> Camera;
+  typedef CameraSet<Camera> Set;
   typedef vector<Point2> ZZ;
-  CameraSet<Camera> set;
+  Set set;
   Camera camera;
   set.push_back(camera);
   set.push_back(camera);
   Point3 p(0, 0, 1);
   EXPECT(assert_equal(set, set));
-  CameraSet<Camera> set2 = set;
+  Set set2 = set;
   set2.push_back(camera);
   EXPECT(!set.equals(set2));
 
@@ -50,7 +51,7 @@ TEST(CameraSet, Pinhole) {
 
   // Calculate expected derivatives using Pinhole
   Matrix43 actualE;
-  Matrix F1;
+  Matrix29 F1;
   {
     Matrix23 E1;
     Matrix23 H1;
@@ -59,12 +60,12 @@ TEST(CameraSet, Pinhole) {
   }
 
   // Check computed derivatives
-  CameraSet<Camera>::FBlocks F;
+  Set::FBlocks Fs;
-  Matrix E, H;
+  Matrix E;
-  set.project2(p, F, E);
+  set.project2(p, Fs, E);
-  LONGS_EQUAL(2,F.size());
+  LONGS_EQUAL(2, Fs.size());
-  EXPECT(assert_equal(F1, F[0]));
+  EXPECT(assert_equal(F1, Fs[0]));
-  EXPECT(assert_equal(F1, F[1]));
+  EXPECT(assert_equal(F1, Fs[1]));
   EXPECT(assert_equal(actualE, E));
 
   // Check errors
@@ -78,6 +79,40 @@ TEST(CameraSet, Pinhole) {
   Vector actualV = set.reprojectionError(p, measured);
   EXPECT(assert_equal(expectedV, actualV));
 
+  // Check Schur complement
+  Matrix F(4, 18);
+  F << F1, Matrix29::Zero(), Matrix29::Zero(), F1;
+  Matrix Ft = F.transpose();
+  Matrix34 Et = E.transpose();
+  Matrix3 P = Et * E;
+  Matrix schur(19, 19);
+  Vector4 b = actualV;
+  Vector v = Ft * (b - E * P * Et * b);
+  schur << Ft * F - Ft * E * P * Et * F, v, v.transpose(), 30;
+  SymmetricBlockMatrix actualReduced = Set::SchurComplement(Fs, E, P, b);
+  EXPECT(assert_equal(schur, actualReduced.matrix()));
+
+  // Check Schur complement update, same order, should just double
+  FastVector<Key> allKeys, keys;
+  allKeys.push_back(1);
+  allKeys.push_back(2);
+  keys.push_back(1);
+  keys.push_back(2);
+  Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys, actualReduced);
+  EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.matrix()));
+
+  // Check Schur complement update, keys reversed
+  FastVector<Key> keys2;
+  keys2.push_back(2);
+  keys2.push_back(1);
+  Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys2, actualReduced);
+  Vector4 reverse_b;
+  reverse_b << b.tail<2>(), b.head<2>();
+  Vector reverse_v = Ft * (reverse_b - E * P * Et * reverse_b);
+  Matrix A(19, 19);
+  A << Ft * F - Ft * E * P * Et * F, reverse_v, reverse_v.transpose(), 30;
+  EXPECT(assert_equal((Matrix )(2.0 * schur + A), actualReduced.matrix()));
+
   // reprojectionErrorAtInfinity
   EXPECT(
       assert_equal(Point3(0, 0, 1),
@@ -113,12 +148,12 @@ TEST(CameraSet, Stereo) {
   }
 
   // Check computed derivatives
-  CameraSet<StereoCamera>::FBlocks F;
+  CameraSet<StereoCamera>::FBlocks Fs;
   Matrix E;
-  set.project2(p, F, E);
+  set.project2(p, Fs, E);
-  LONGS_EQUAL(2,F.size());
+  LONGS_EQUAL(2, Fs.size());
-  EXPECT(assert_equal(F1, F[0]));
+  EXPECT(assert_equal(F1, Fs[0]));
-  EXPECT(assert_equal(F1, F[1]));
+  EXPECT(assert_equal(F1, Fs[1]));
   EXPECT(assert_equal(actualE, E));
 }