From 6d118da82de954d11ca8b4a0505845059804c5c6 Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Sun, 14 Mar 2021 22:43:53 -0400
Subject: [PATCH] still segfaults

---
 gtsam/geometry/CameraSet.h                    |  9 +++--
 .../slam/SmartStereoProjectionFactorPP.h      | 38 ++++++++++++-------
 2 files changed, 30 insertions(+), 17 deletions(-)

diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h
index 6aaa34c14..80f6b2305 100644
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@@ -146,13 +146,14 @@ public:
      * Fixed size version
      */
     template<int N, int ND> // N = 2 or 3, ND is the camera dimension
-    static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs,
+    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) {
 
       // a single point is observed in m cameras
       size_t m = Fs.size();
 
-      // Create a SymmetricBlockMatrix
+      // 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);
@@ -162,7 +163,7 @@ public:
       // Blockwise Schur complement
       for (size_t i = 0; i < m; i++) { // for each camera
 
-        const MatrixZD& Fi = Fs[i];
+        const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
         const auto FiT = Fi.transpose();
         const Eigen::Matrix<double, ZDim, N> Ei_P = //
             E.block(ZDim * i, 0, ZDim, N) * P;
@@ -177,7 +178,7 @@ public:
 
         // upper triangular part of the hessian
         for (size_t j = i + 1; j < m; j++) { // for each camera
-          const MatrixZD& Fj = Fs[j];
+          const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
 
           // (DxD) = (Dx2) * ( (2x2) * (2xD) )
           augmentedHessian.setOffDiagonalBlock(i, j, -FiT
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
index 8374e079a..3cbb4af30 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
@@ -181,7 +181,14 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
       const Values& values, const double lambda = 0.0,  bool diagonalDamping =
           false) const {
 
-    size_t numKeys = this->keys_.size();
+    KeyVector allKeys; // includes body poses and *unique* extrinsic poses
+    allKeys.insert(allKeys.end(), this->keys_.begin(), this->keys_.end());
+    KeyVector sorted_body_P_cam_keys(body_P_cam_keys_); // make a copy that we can edit
+    std::sort(sorted_body_P_cam_keys.begin(), sorted_body_P_cam_keys.end()); // required by unique
+    std::unique(sorted_body_P_cam_keys.begin(), sorted_body_P_cam_keys.end());
+    allKeys.insert(allKeys.end(), sorted_body_P_cam_keys.begin(), sorted_body_P_cam_keys.end());
+    size_t numKeys = allKeys.size();
+
     // Create structures for Hessian Factors
     KeyVector js;
     std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
@@ -202,7 +209,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
         m = Matrix::Zero(Dim,Dim);
       for(Vector& v: gs)
         v = Vector::Zero(Dim);
-      return boost::make_shared<RegularHessianFactor<Dim> >(this->keys_,
+      return boost::make_shared<RegularHessianFactor<Dim> >(allKeys,
                                                                   Gs, gs, 0.0);
     }
 
@@ -214,17 +221,22 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
     computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
     std::cout << "Fs.at(0) \n"<< Fs.at(0) << std::endl;
 
-//    // Whiten using noise model
-//    Base::whitenJacobians(Fs, E, b);
-//
-//    // build augmented hessian
-//    SymmetricBlockMatrix augmentedHessian = //
-//        Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping);
-//
-//    return boost::make_shared<RegularHessianFactor<Dim> >(this->keys_,
-//                                                                augmentedHessian);
-    return boost::make_shared<RegularHessianFactor<Dim> >(this->keys_,
-                                                                      Gs, gs, 0.0);
+    std::cout << "Dim "<< Dim << std::endl;
+    std::cout << "numKeys "<< numKeys << std::endl;
+
+    // Whiten using noise model
+    noiseModel_->WhitenSystem(E, b);
+    for (size_t i = 0; i < Fs.size(); i++)
+      Fs[i] = noiseModel_->Whiten(Fs[i]);
+
+    // build augmented hessian
+    Matrix3 P;
+    Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
+    SymmetricBlockMatrix augmentedHessian = //
+        Cameras::SchurComplement<3,Dim>(Fs, E, P, b);
+
+    return boost::make_shared<RegularHessianFactor<Dim> >(allKeys,
+                                                                augmentedHessian);
   }
 
   /**