From 477dd5b247d0246c6efc609f62580a1d1db8b412 Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Thu, 22 Jul 2021 21:49:40 -0400
Subject: [PATCH] all pass!

---
 gtsam/geometry/CameraSet.h                    | 58 ++-----------------
 .../SmartProjectionPoseFactorRollingShutter.h |  9 +--
 .../slam/SmartStereoProjectionFactorPP.h      |  2 +-
 3 files changed, 8 insertions(+), 61 deletions(-)

diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h
index fcbff020c..2b656bd35 100644
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@@ -142,57 +142,11 @@ public:
     return ErrorVector(project2(point, Fs, E), measured);
   }
 
-  static SymmetricBlockMatrix mySchurComplement(
-        const std::vector< Eigen::Matrix<double, 2, 12>,
-        Eigen::aligned_allocator< Eigen::Matrix<double, 2, 12> > >& Fs,
+  static SymmetricBlockMatrix SchurComplement312(
+        const std::vector< Eigen::Matrix<double, ZDim, 12>,
+        Eigen::aligned_allocator< Eigen::Matrix<double, ZDim, 12> > >& Fs,
         const Matrix& E, const Eigen::Matrix<double, 3, 3>& P, const Vector& b) {
-    return mySchurComplement<2,3,12>(Fs, E, P, b);
-  }
-
-  template<int myZDim, int N, int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
-  static SymmetricBlockMatrix mySchurComplement(
-      const std::vector< Eigen::Matrix<double, myZDim, ND>,
-      Eigen::aligned_allocator< Eigen::Matrix<double, myZDim, ND> > >& Fs,
-      const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
-
-    // a single point is observed in m cameras
-    size_t m = Fs.size();
-
-    // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
-    size_t M1 = ND * m + 1;
-    std::vector<DenseIndex> dims(m + 1); // this also includes the b term
-    std::fill(dims.begin(), dims.end() - 1, ND);
-    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 Eigen::Matrix<double, myZDim, ND>& Fi = Fs[i];
-      const auto FiT = Fi.transpose();
-      const Eigen::Matrix<double, myZDim, N> Ei_P = //
-          E.block(myZDim * i, 0, myZDim, N) * P;
-
-      // D = (Dx2) * ZDim
-      augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<myZDim>(myZDim * i) // F' * b
-                                           - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
-
-      // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
-      augmentedHessian.setDiagonalBlock(i, FiT
-                                        * (Fi - Ei_P * E.block(myZDim * i, 0, myZDim, N).transpose() * Fi));
-
-      // upper triangular part of the hessian
-      for (size_t j = i + 1; j < m; j++) { // for each camera
-        const Eigen::Matrix<double, myZDim, ND>& Fj = Fs[j];
-
-        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
-        augmentedHessian.setOffDiagonalBlock(i, j, -FiT
-                                             * (Ei_P * E.block(myZDim * j, 0, myZDim, N).transpose() * Fj));
-      }
-    } // end of for over cameras
-
-    augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
-    return augmentedHessian;
+    return SchurComplement<3,12>(Fs, E, P, b);
   }
 
   /**
@@ -202,7 +156,7 @@ public:
      * Fixed size version
      */
     template<int N, int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
-    static SymmetricBlockMatrix SchurComplementWithCustomBlocks(
+    static SymmetricBlockMatrix SchurComplement(
         const std::vector< Eigen::Matrix<double, ZDim, ND>, Eigen::aligned_allocator< Eigen::Matrix<double, ZDim, ND> > >& Fs,
         const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
 
@@ -255,7 +209,7 @@ public:
   template<int N> // N = 2 or 3
   static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs,
       const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
-    return SchurComplementWithCustomBlocks<N,D>(Fs, E, P, b);
+    return SchurComplement<N,D>(Fs, E, P, b);
   }
 
   /// Computes Point Covariance P, with lambda parameter
diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
index 472b6348a..3393f7ca4 100644
--- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
+++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
@@ -342,7 +342,7 @@ PinholePose<CALIBRATION> > {
     //     Base::Cameras::SchurComplement(Fs, E, b, lambda,diagonalDamping);
 
     SymmetricBlockMatrix augmentedHessian =
-         Base::Cameras::mySchurComplement(Fs, E, P, b);
+         Base::Cameras::SchurComplement312(Fs, E, P, b);
 
     // now pack into an Hessian factor
     std::vector<DenseIndex> dims(nrUniqueKeys + 1);  // this also includes the b term
@@ -415,13 +415,6 @@ PinholePose<CALIBRATION> > {
     }
     return boost::make_shared < RegularHessianFactor<DimPose>
     > ( this->keys_, augmentedHessianUniqueKeys);
-
-//    // TO REMOVE:
-//    for (Matrix& m : Gs)
-//      m = Matrix::Zero(DimPose, DimPose);
-//    for (Vector& v : gs)
-//      v = Vector::Zero(DimPose);
-//    return boost::make_shared < RegularHessianFactor<DimPose> > ( this->keys_, Gs, gs, 0.0);
   }
 
   /**
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
index e4d714b0f..40d90d614 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
@@ -250,7 +250,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
 
     // marginalize point: note - we reuse the standard SchurComplement function
     SymmetricBlockMatrix augmentedHessian =
-        Cameras::SchurComplementWithCustomBlocks<3, Dim>(Fs, E, P, b);
+        Cameras::SchurComplement<3, Dim>(Fs, E, P, b);
 
     // now pack into an Hessian factor
     std::vector<DenseIndex> dims(nrUniqueKeys + 1);  // this also includes the b term